ComputeDiagonalSingularValues()
{
return(Matrix.Diagonal(_singular));
}
private void UpdateTimeVaryingMatrices(TimeSpan timeDelta)
{
double dt = timeDelta.TotalSeconds;
double posByV = dt; // p=vt
double posByA = 0.5 * dt * dt; // p=0.5at^2
double velByA = dt; // v=at
// World state transition matrix.
// Update position and velocity from previous state.
// Previous state acceleration is neglected since current acceleration only depends on current input.
A = Matrix.Create(new double[n, n]
{
// Px Py Pz Vx Vy Vz Qx Qy Qz Qw
{ 1, 0, 0, posByV, 0, 0, 0, 0, 0, 0 }, // Px
{ 0, 1, 0, 0, posByV, 0, 0, 0, 0, 0 }, // Py
{ 0, 0, 1, 0, 0, posByV, 0, 0, 0, 0 }, // Pz
{ 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 }, // Vx
{ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, // Vy
{ 0, 0, 0, 0, 0, 1, 0, 0, 0, 0 }, // Vz
// We don't handle transition of quaternions here due to difficulties. Using B instead.
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Quaternion X
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Quaternion Y
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Quaternion Z
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, // Quaternion W
});
AT = Matrix.Transpose(A);
// Input gain matrix.
// Acceleration forward/right/down
// Angular Rate Roll/Pitch/Heading
B = Matrix.Create(new double[n, p]
{
// Ax Ay Az Qx Qy Qz Qw
{ posByA, 0, 0, 0, 0, 0, 0 }, // Px
{ 0, posByA, 0, 0, 0, 0, 0 }, // Py
{ 0, 0, posByA, 0, 0, 0, 0 }, // Pz
{ velByA, 0, 0, 0, 0, 0, 0 }, // Vx
{ 0, velByA, 0, 0, 0, 0, 0 }, // Vy
{ 0, 0, velByA, 0, 0, 0, 0 }, // Vz
// Simply set new orientation directly by quaternion input
{ 0, 0, 0, 1, 0, 0, 0 }, // Quaternion X
{ 0, 0, 0, 0, 1, 0, 0 }, // Quaternion Y
{ 0, 0, 0, 0, 0, 1, 0 }, // Quaternion Z
{ 0, 0, 0, 0, 0, 0, 1 }, // Quaternion W
});
// TODO For simplicity we assume all acceleromter axes have identical standard deviations (although they don't)
float accelStdDev = _sensorSpecifications.AccelerometerStdDev.Forward;
float velocityStdDev = ((float)velByA) * accelStdDev;
float positionStdDev = ((float)posByA) * accelStdDev;
// Diagonal matrix with noise std dev
Q = Matrix.Diagonal(new Vector(new double[n]
{
positionStdDev, positionStdDev, positionStdDev,
velocityStdDev, velocityStdDev, velocityStdDev,
0, 0, 0, 0 // TODO Orientation has no noise, should be added later
}));
R = Matrix.Diagonal(_stdDevV); // Diagonal matrix with noise std dev
Q *= Matrix.Transpose(Q); // Convert standard deviations to variances
R *= Matrix.Transpose(R); // Convert standard deviations to variances
w = GetNoiseMatrix(_stdDevW, n);
v = GetNoiseMatrix(_stdDevV, m);
}