diff --git a/Amethyst.Plugins.Contract/Amethyst.Plugins.Contract.csproj b/Amethyst.Plugins.Contract/Amethyst.Plugins.Contract.csproj index 96902b6f..27fdacdf 100644 --- a/Amethyst.Plugins.Contract/Amethyst.Plugins.Contract.csproj +++ b/Amethyst.Plugins.Contract/Amethyst.Plugins.Contract.csproj @@ -7,13 +7,13 @@ True Amethyst Device Plugin API (Contract) - 0.2.11 + 0.2.13 x64 Amethyst.Plugins.Contract - 0.2.22 + 0.2.23 K2VR K2VR Amethyst;K2VR;KinectToVR;VR;OpenVR;Fullbody Tracking diff --git a/Amethyst.Plugins.Contract/Classes.cs b/Amethyst.Plugins.Contract/Classes.cs index 45413022..66fde81f 100644 --- a/Amethyst.Plugins.Contract/Classes.cs +++ b/Amethyst.Plugins.Contract/Classes.cs @@ -121,6 +121,12 @@ public TrackerBase() [DataMember(Name = "ConnectionState")] public bool ConnectionState { get; set; } = false; + /// + /// Tracking state: tracked/occluded/not tracked + /// + [DataMember(Name = "TrackingState")] + public TrackedJointState TrackingState { get; set; } = TrackedJointState.StateNotTracked; + /// /// Serial number, or name, identifies a tracker /// diff --git a/Amethyst.Support/Amethyst.Support.vcxproj b/Amethyst.Support/Amethyst.Support.vcxproj index 3cf89a46..9e858f41 100644 --- a/Amethyst.Support/Amethyst.Support.vcxproj +++ b/Amethyst.Support/Amethyst.Support.vcxproj @@ -129,7 +129,6 @@ - Support.idl diff --git a/Amethyst.Support/KalmanFilter.h b/Amethyst.Support/KalmanFilter.h deleted file mode 100644 index 1ab7837d..00000000 --- a/Amethyst.Support/KalmanFilter.h +++ /dev/null @@ -1,138 +0,0 @@ -#pragma once -#include - -class CKalmanFilter -{ -public: - /** - * Create a Kalman filter with the specified matrices. - * A - System dynamics matrix - * B - Input matrix - * C - Output matrix - * Q - Process noise covariance - * R - Measurement noise covariance - * P - Estimate error covariance - */ - - CKalmanFilter() - { - constexpr int _n = 3; // Number of states - constexpr int _m = 1; // Number of measurements - constexpr int _c = 1; // Number of control inputs - - Eigen::MatrixXf _A(_n, _n); // System dynamics matrix - Eigen::MatrixXf _B(_n, _c); // Input control matrix - Eigen::MatrixXf _C(_m, _n); // Output matrix - Eigen::MatrixXf _Q(_n, _n); // Process noise covariance - Eigen::MatrixXf _R(_m, _m); // Measurement noise covariance - Eigen::MatrixXf _P(_n, _n); // Estimate error covariance - - double dt = 1.0 / 100; - // Discrete LTI projectile motion, measuring position only - _A << 1, dt, 0, 0, 1, dt, 0, 0, 1; - _B << 0, 0, 0; - _C << 1, 0, 0; - - // Reasonable covariance matrices - _Q << .17, .17, .0, .17, .17, .0, .0, .0, .0; - _R << 5; - _P << .3, .3, .3, .3, 30000, 30, .3, 30, 300; - - A = _A; - B = _B; - C = _C; - Q = _Q; - R = _R; - - P0 = _P; - - m = _C.rows(); - n = _A.rows(); - c = _B.cols(); - - I = Eigen::MatrixXf(n, n); - x_hat = Eigen::VectorXf(n); - - initialized = false; - I.setIdentity(); - } - - /** - * Initialize the filter with initial states as zero. - */ - void init() - { - x_hat.setZero(); - P = P0; - initialized = true; - } - - /** - * Initialize the filter with a guess for initial states. - */ - void init(const Eigen::VectorXf& x0) - { - x_hat = x0; - P = P0; - initialized = true; - } - - /** - * Update the prediction based on control input. - */ - void predict(const Eigen::VectorXf& u) - { - if (!initialized) - init(); - - x_hat = A * x_hat + B * u; - P = A * P * A.transpose() + Q; - } - - /** - * Update the estimated state based on measured values. - */ - void update(const Eigen::VectorXf& y) - { - K = P * C.transpose() * (C * P * C.transpose() + R).inverse(); - x_hat += K * (y - C * x_hat); - P = (I - K * C) * P; - } - - /** - * Update the dynamics matrix. - */ - void update_dynamics(const Eigen::MatrixXf A) - { - this->A = A; - } - - /** - * Update the output matrix. - */ - void update_output(const Eigen::MatrixXf C) - { - this->C = C; - } - - /** - * Return the current state. - */ - Eigen::VectorXf state() { return x_hat; }; - -private: - // Matrices for computation - Eigen::MatrixXf A, B, C, Q, R, P, K, P0; - - // System dimensions - int m, n, c; - - // Is the filter initialized? - bool initialized = false; - - // n-size identity - Eigen::MatrixXf I; - - // Estimated states - Eigen::VectorXf x_hat; -}; diff --git a/Amethyst.Support/Support.cpp b/Amethyst.Support/Support.cpp index aceb51db..80a3a51a 100644 --- a/Amethyst.Support/Support.cpp +++ b/Amethyst.Support/Support.cpp @@ -3,9 +3,8 @@ #include "Support.g.cpp" // NOLINT(bugprone-suspicious-include) #include "LowPassFilter.g.cpp" // NOLINT(bugprone-suspicious-include) -#include "KalmanFilter.g.cpp" // NOLINT(bugprone-suspicious-include) -#include "KalmanFilter.h" +#include #include // The upper bound of the fog volume along the y-axis (height) @@ -344,39 +343,4 @@ namespace winrt::AmethystSupport::implementation return output_; // Update and return } - - KalmanFilter::KalmanFilter() - { - for (auto& filter : filters_) - filter.init(); // Setup! - } - - SVector3 KalmanFilter::Update(const SVector3& input) - { - Eigen::VectorXf u(1); - Eigen::VectorXf y[3] = - { - Eigen::VectorXf(1), - Eigen::VectorXf(1), - Eigen::VectorXf(1) - }; - - y[0] << input.X; - y[1] << input.Y; - y[2] << input.Z; - u << 0; // zero control input - - for (auto& filter : filters_) - filter.predict(u); - - filters_[0].update(y[0]); - filters_[1].update(y[1]); - filters_[2].update(y[2]); - - return SVector3{ - .X = filters_[0].state().x(), - .Y = filters_[1].state().x(), - .Z = filters_[2].state().x() - }; - } } diff --git a/Amethyst.Support/Support.h b/Amethyst.Support/Support.h index d95d7370..32183d3a 100644 --- a/Amethyst.Support/Support.h +++ b/Amethyst.Support/Support.h @@ -1,7 +1,5 @@ #pragma once #include "Support.g.h" -#include "KalmanFilter.g.h" -#include "KalmanFilter.h" #include "LowPassFilter.g.h" namespace winrt::AmethystSupport::implementation @@ -29,16 +27,6 @@ namespace winrt::AmethystSupport::implementation SVector3 output_{0, 0, 0}; float e_pow_; // Init-only }; - - struct KalmanFilter : KalmanFilterT - { - KalmanFilter(); - - SVector3 Update(const SVector3& input); - - private: - std::array filters_; - }; } namespace winrt::AmethystSupport::factory_implementation @@ -50,8 +38,4 @@ namespace winrt::AmethystSupport::factory_implementation struct LowPassFilter : LowPassFilterT { }; - - struct KalmanFilter : KalmanFilterT - { - }; } diff --git a/Amethyst.Support/Support.idl b/Amethyst.Support/Support.idl index 2eb229e3..2a1ad424 100644 --- a/Amethyst.Support/Support.idl +++ b/Amethyst.Support/Support.idl @@ -40,11 +40,4 @@ namespace AmethystSupport LowPassFilter(Single cutOff, Single deltaTime); SVector3 Update(SVector3 input); // Recalculate } - - [default_interface] - runtimeclass KalmanFilter - { - KalmanFilter(); // No explc parameters required - SVector3 Update(SVector3 input); // Recalculate - } } diff --git a/Amethyst.sln b/Amethyst.sln index e0a65bc0..ef9e580b 100644 --- a/Amethyst.sln +++ b/Amethyst.sln @@ -10,60 +10,50 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "Amethyst.Support", "Amethys EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution - Debug|ARM = Debug|ARM Debug|ARM64 = Debug|ARM64 Debug|x64 = Debug|x64 Debug|x86 = Debug|x86 - Release|ARM = Release|ARM Release|ARM64 = Release|ARM64 Release|x64 = Release|x64 Release|x86 = Release|x86 EndGlobalSection GlobalSection(ProjectConfigurationPlatforms) = postSolution - {4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|ARM.ActiveCfg = Debug|x64 - {4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|ARM.Build.0 = Debug|x64 {4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|ARM64.ActiveCfg = Debug|x64 {4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|ARM64.Build.0 = Debug|x64 + {4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|ARM64.Deploy.0 = Debug|x64 {4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|x64.ActiveCfg = Debug|x64 {4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|x64.Build.0 = Debug|x64 {4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|x64.Deploy.0 = Debug|x64 {4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|x86.ActiveCfg = Debug|x64 {4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|x86.Build.0 = Debug|x64 - {4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|ARM.ActiveCfg = Release|x64 - {4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|ARM.Build.0 = Release|x64 + {4204FCE6-75A0-4348-B401-F283631F1A7D}.Debug|x86.Deploy.0 = Debug|x64 {4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|ARM64.ActiveCfg = Release|x64 {4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|ARM64.Build.0 = Release|x64 + {4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|ARM64.Deploy.0 = Release|x64 {4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|x64.ActiveCfg = Release|x64 {4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|x64.Build.0 = Release|x64 {4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|x64.Deploy.0 = Release|x64 {4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|x86.ActiveCfg = Release|x64 {4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|x86.Build.0 = Release|x64 - {8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Debug|ARM.ActiveCfg = Debug|x64 - {8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Debug|ARM.Build.0 = Debug|x64 + {4204FCE6-75A0-4348-B401-F283631F1A7D}.Release|x86.Deploy.0 = Release|x64 {8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Debug|ARM64.ActiveCfg = Debug|x64 {8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Debug|ARM64.Build.0 = Debug|x64 {8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Debug|x64.ActiveCfg = Debug|x64 {8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Debug|x64.Build.0 = Debug|x64 {8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Debug|x86.ActiveCfg = Debug|x64 {8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Debug|x86.Build.0 = Debug|x64 - {8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Release|ARM.ActiveCfg = Release|x64 - {8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Release|ARM.Build.0 = Release|x64 {8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Release|ARM64.ActiveCfg = Release|x64 {8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Release|ARM64.Build.0 = Release|x64 {8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Release|x64.ActiveCfg = Release|x64 {8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Release|x64.Build.0 = Release|x64 {8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Release|x86.ActiveCfg = Release|x64 {8F6A83EB-577D-41AA-B348-0E41A4CEF186}.Release|x86.Build.0 = Release|x64 - {FD15CD75-291A-4911-BFBC-32E7381034CF}.Debug|ARM.ActiveCfg = Debug|ARM - {FD15CD75-291A-4911-BFBC-32E7381034CF}.Debug|ARM.Build.0 = Debug|ARM {FD15CD75-291A-4911-BFBC-32E7381034CF}.Debug|ARM64.ActiveCfg = Debug|ARM64 {FD15CD75-291A-4911-BFBC-32E7381034CF}.Debug|ARM64.Build.0 = Debug|ARM64 {FD15CD75-291A-4911-BFBC-32E7381034CF}.Debug|x64.ActiveCfg = Debug|x64 {FD15CD75-291A-4911-BFBC-32E7381034CF}.Debug|x64.Build.0 = Debug|x64 {FD15CD75-291A-4911-BFBC-32E7381034CF}.Debug|x86.ActiveCfg = Debug|Win32 {FD15CD75-291A-4911-BFBC-32E7381034CF}.Debug|x86.Build.0 = Debug|Win32 - {FD15CD75-291A-4911-BFBC-32E7381034CF}.Release|ARM.ActiveCfg = Release|ARM - {FD15CD75-291A-4911-BFBC-32E7381034CF}.Release|ARM.Build.0 = Release|ARM {FD15CD75-291A-4911-BFBC-32E7381034CF}.Release|ARM64.ActiveCfg = Release|ARM64 {FD15CD75-291A-4911-BFBC-32E7381034CF}.Release|ARM64.Build.0 = Release|ARM64 {FD15CD75-291A-4911-BFBC-32E7381034CF}.Release|x64.ActiveCfg = Release|x64 diff --git a/Amethyst/Amethyst.csproj b/Amethyst/Amethyst.csproj index 7a7f2c34..215eb98b 100644 --- a/Amethyst/Amethyst.csproj +++ b/Amethyst/Amethyst.csproj @@ -55,6 +55,7 @@ + diff --git a/Amethyst/Classes/AppSettings.cs b/Amethyst/Classes/AppSettings.cs index 00841e23..f16cb316 100644 --- a/Amethyst/Classes/AppSettings.cs +++ b/Amethyst/Classes/AppSettings.cs @@ -543,8 +543,8 @@ public static bool? ExtraTrackers public static bool SetupFinished { - get => Windows.Storage.ApplicationData.Current.LocalSettings.Values["SetupFinished"] as bool? ?? false; - set => Windows.Storage.ApplicationData.Current.LocalSettings.Values["SetupFinished"] = value; + get => Windows.Storage.ApplicationData.Current.LocalSettings.Values["FirstSetupFinished"] as bool? ?? false; + set => Windows.Storage.ApplicationData.Current.LocalSettings.Values["FirstSetupFinished"] = value; } } diff --git a/Amethyst/Classes/AppTracker.cs b/Amethyst/Classes/AppTracker.cs index 0942e0b2..6557729f 100644 --- a/Amethyst/Classes/AppTracker.cs +++ b/Amethyst/Classes/AppTracker.cs @@ -80,6 +80,7 @@ public class AppTracker : INotifyPropertyChanged [JsonIgnore] public long PoseTimestamp { get; set; } = 0; [JsonIgnore] public long PreviousPoseTimestamp { get; set; } = 0; + [JsonIgnore] public TrackedJointState TrackingState { get; set; } = TrackedJointState.StateNotTracked; // Is this joint overridden? public bool IsPositionOverridden @@ -626,6 +627,7 @@ public TrackerBase GetTrackerBase( var trackerBase = new TrackerBase { ConnectionState = IsActive, + TrackingState = TrackingState, Role = Role, Serial = Serial, @@ -683,7 +685,7 @@ public void UpdateFilters() { // Update LowPass and Kalman filters _lowPassPosition = _lowPassFilter.Update(Position.Projected()).V(); - _kalmanPosition = _kalmanFilter.Update(Position.Projected()).V(); + _kalmanPosition = _kalmanFilter.Update(Position); // Update the LERP (mix) filter _lerpPosition = Vector3.Lerp(_lastLerpPosition, Position, 0.31f); diff --git a/Amethyst/Classes/Main.cs b/Amethyst/Classes/Main.cs index 8fa44b17..ca514c6d 100644 --- a/Amethyst/Classes/Main.cs +++ b/Amethyst/Classes/Main.cs @@ -402,6 +402,7 @@ private static void UpdateAppTrackers() // Push raw positions and timestamps tracker.Position = joint.Position; + tracker.TrackingState = joint.TrackingState; tracker.PreviousPosition = joint.PreviousPosition; tracker.PoseTimestamp = joint.PoseTimestamp; @@ -496,6 +497,7 @@ private static void UpdateAppTrackers() { // Push raw positions and timestamps tracker.Position = joint.Position; + tracker.TrackingState = joint.TrackingState; tracker.PreviousPosition = joint.PreviousPosition; tracker.PoseTimestamp = joint.PoseTimestamp; diff --git a/Amethyst/MVVM/ServiceEndpoint.cs b/Amethyst/MVVM/ServiceEndpoint.cs index c3c356a5..79d416d3 100644 --- a/Amethyst/MVVM/ServiceEndpoint.cs +++ b/Amethyst/MVVM/ServiceEndpoint.cs @@ -11,7 +11,8 @@ namespace Amethyst.MVVM; -public class ServiceEndpoint(string name, string guid, string path, Version version, IServiceEndpoint service) : INotifyPropertyChanged +public class ServiceEndpoint(string name, string guid, string path, Version version, IServiceEndpoint service) + : INotifyPropertyChanged { // Extensions: is this service used atm? public bool IsUsed => AppData.Settings.ServiceEndpointGuid == Guid; @@ -181,7 +182,14 @@ public void Initialize() // This is called when the service is closed public void Shutdown() { - Service.Shutdown(); + try + { + Service.Shutdown(); + } + catch (Exception ex) + { + Logger.Error(ex); + } // Try to cleanup memory after the plugin GC.Collect(GC.MaxGeneration, GCCollectionMode.Aggressive, true, true); diff --git a/Amethyst/Utils/KalmanFilter.cs b/Amethyst/Utils/KalmanFilter.cs new file mode 100644 index 00000000..dcc7b0fc --- /dev/null +++ b/Amethyst/Utils/KalmanFilter.cs @@ -0,0 +1,185 @@ +using System; +using System.Numerics; +using MathNet.Numerics.LinearAlgebra; + +namespace Amethyst.Utils; + +public class KalmanFilter +{ + private Vector3 _position; + + private SingularKalmanFilter[] Filters { get; } = new[] + { new SingularKalmanFilter(), new SingularKalmanFilter(), new SingularKalmanFilter() }; + + public Vector3 Update(Vector3 input) + { + _position.X = Filters[0].Update(input.X); + _position.Y = Filters[1].Update(input.Y); + _position.Z = Filters[2].Update(input.Z); + return _position; // Don't create 'new' vectors + } +} + +internal class SingularKalmanFilter +{ + private int L, m; + private float alpha, ki, beta, lambda, c, q, r; + private Matrix Wm, Wc, x, P, Q, R; + + /// + /// Constructor of Unscented Kalman Filter + /// + /// States number + /// Measurements number + public SingularKalmanFilter(int L = 0) + { + this.L = L; + } + + private void Init() + { + q = 0.05f; + r = 0.3f; + + x = q * Matrix.Build.Random(L, 1); //initial state with noise + P = Matrix.Build.Diagonal(L, L, 1); //initial state covraiance + + Q = Matrix.Build.Diagonal(L, L, q * q); //covariance of process + R = Matrix.Build.Dense(m, m, r * r); //covariance of measurement + + alpha = 1e-3f; + ki = 0; + beta = 2f; + lambda = alpha * alpha * (L + ki) - L; + c = L + lambda; + + //weights for means + Wm = Matrix.Build.Dense(1, 2 * L + 1, 0.5f / c); + Wm[0, 0] = lambda / c; + + //weights for covariance + Wc = Matrix.Build.Dense(1, 2 * L + 1); + Wm.CopyTo(Wc); + Wc[0, 0] = Wm[0, 0] + 1 - alpha * alpha + beta; + + c = MathF.Sqrt(c); + } + + public float Update(float measurement) + { + if (m == 0) + { + m = 1; + if (L == 0) L = 1; + Init(); + } + + var z = Matrix.Build.Dense(m, 1, 0); + z[0, 0] = measurement; + + //sigma points around x + var X = GetSigmaPoints(x, P, c); + + //unscented transformation of process + // X1=sigmas(x1,P1,c) - sigma points around x1 + //X2=X1-x1(:,ones(1,size(X1,2))) - deviation of X1 + var ut_f_matrices = UnscentedTransform(X, Wm, Wc, L, Q); + var x1 = ut_f_matrices[0]; + var X1 = ut_f_matrices[1]; + var P1 = ut_f_matrices[2]; + var X2 = ut_f_matrices[3]; + + //unscented transformation of measurments + var ut_h_matrices = UnscentedTransform(X1, Wm, Wc, m, R); + var z1 = ut_h_matrices[0]; + var Z1 = ut_h_matrices[1]; + var P2 = ut_h_matrices[2]; + var Z2 = ut_h_matrices[3]; + + //transformed cross-covariance + var P12 = X2.Multiply(Matrix.Build.Diagonal(Wc.Row(0).ToArray())).Multiply(Z2.Transpose()); + + var K = P12.Multiply(P2.Inverse()); + + //state update + x = x1.Add(K.Multiply(z.Subtract(z1))); + //covariance update + P = P1.Subtract(K.Multiply(P12.Transpose())); + return GetState()[0]; + } + + public float[] GetState() + { + return x.ToColumnArrays()[0]; + } + + public float[,] GetCovariance() + { + return P.ToArray(); + } + + /// + /// Unscented Transformation + /// + /// nonlinear map + /// sigma points + /// Weights for means + /// Weights for covariance + /// numer of outputs of f + /// additive covariance + /// [transformed mean, transformed smapling points, transformed covariance, transformed deviations + private Matrix[] UnscentedTransform(Matrix X, Matrix Wm, Matrix Wc, int n, + Matrix R) + { + var L = X.ColumnCount; + var y = Matrix.Build.Dense(n, 1, 0); + var Y = Matrix.Build.Dense(n, L, 0); + + Matrix row_in_X; + for (var k = 0; k < L; k++) + { + row_in_X = X.SubMatrix(0, X.RowCount, k, 1); + Y.SetSubMatrix(0, Y.RowCount, k, 1, row_in_X); + y = y.Add(Y.SubMatrix(0, Y.RowCount, k, 1).Multiply(Wm[0, k])); + } + + var Y1 = Y.Subtract(y.Multiply(Matrix.Build.Dense(1, L, 1))); + var P = Y1.Multiply(Matrix.Build.Diagonal(Wc.Row(0).ToArray())); + P = P.Multiply(Y1.Transpose()); + P = P.Add(R); + + Matrix[] output = { y, Y, P, Y1 }; + return output; + } + + /// + /// Sigma points around reference point + /// + /// reference point + /// covariance + /// coefficient + /// Sigma points + private Matrix GetSigmaPoints(Matrix x, Matrix P, float c) + { + var A = P.Cholesky().Factor; + + A = A.Multiply(c); + A = A.Transpose(); + + var n = x.RowCount; + + var Y = Matrix.Build.Dense(n, n, 1); + for (var j = 0; j < n; j++) Y.SetSubMatrix(0, n, j, 1, x); + + var X = Matrix.Build.Dense(n, 2 * n + 1); + X.SetSubMatrix(0, n, 0, 1, x); + + var Y_plus_A = Y.Add(A); + X.SetSubMatrix(0, n, 1, n, Y_plus_A); + + var Y_minus_A = Y.Subtract(A); + X.SetSubMatrix(0, n, n + 1, n, Y_minus_A); + + return X; + } +} \ No newline at end of file