UKFSharp
UKFSharp copied to clipboard
Have a question about the UnscentedTrans.
In the method of
private Matrix<double>[] UnscentedTransform(Matrix<double> X, Matrix<double> Wm, Matrix<double> Wc, int n, Matrix<double> R)
{
int L = X.ColumnCount;
Matrix<double> y = Matrix.Build.Dense(n, 1, 0);
Matrix<double> Y = Matrix.Build.Dense(n, L, 0);
Matrix<double> row_in_X;
for (int 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]));
}
Matrix<double> Y1 = Y.Subtract(y.Multiply(Matrix.Build.Dense(1, L, 1)));
Matrix<double> P = Y1.Multiply(Matrix.Build.Diagonal(Wc.Row(0).ToArray()));
P = P.Multiply(Y1.Transpose());
P = P.Add(R);
Matrix<double>[] output = { y, Y, P, Y1 };
return output;
}
Where is the nonlinear transform with X state sigma points, i don't know how to use this class to perform nonlinear transform.
UnscentedTransform is a private method to perform transformation using provied mean and covariance. It relates to the Update method which is public and can be used externally. For what purpose do you want to use the UnscentedTransform?
Thanks your reply, i want to use ukf to integrate my imu device and other sensor, first i would used the imu to calculate the people 2d position by PDR method and integrate measurement position by other sensor like Wifi with RSSI method, so in the UT transform that i want use pdr function as nonlinear function to transform original sigma points.