/// <summary>
/// Fill noise covariance matrix Pnn: this is the covariance of
/// the noise vector (V)
/// (Omega)
/// that gets added to the state.
/// Form of this could change later, but for now assume that
/// V and Omega are independent, and that each of their components is
/// independent...
/// </summary>
/// <param name="xv"></param>
/// <param name="v"></param>
/// <param name="delta_t"></param>
public override void func_Q(Vector xv, Vector v, float delta_t)
{
float linear_velocity_noise_variance =
SD_A_component_filter * SD_A_component_filter * delta_t * delta_t;
float angular_velocity_noise_variance =
SD_alpha_component_filter * SD_alpha_component_filter * delta_t * delta_t;
// Independence means that the matrix is diagonal
MatrixFixed Pnn = new MatrixFixed(6, 6);
Pnn.Fill(0.0f);
Pnn.Put(0, 0, linear_velocity_noise_variance);
Pnn.Put(1, 1, linear_velocity_noise_variance);
Pnn.Put(2, 2, linear_velocity_noise_variance);
Pnn.Put(3, 3, angular_velocity_noise_variance);
Pnn.Put(4, 4, angular_velocity_noise_variance);
Pnn.Put(5, 5, angular_velocity_noise_variance);
// Form Jacobian dxnew_by_dn
// Is like this:
// I * delta_t 0
// 0 dqnew_by_dOmega
// I 0
// 0 I
// Start by zeroing
MatrixFixed dxnew_by_dn = new MatrixFixed(13, 6);
dxnew_by_dn.Fill(0.0f);
// Fill in easy bits first
MatrixFixed Temp33A = new MatrixFixed(3, 3);
Temp33A.SetIdentity();
dxnew_by_dn.Update(Temp33A, 7, 0);
dxnew_by_dn.Update(Temp33A, 10, 3);
Temp33A *= delta_t;
dxnew_by_dn.Update(Temp33A, 0, 0);
// Tricky bit is dqnew_by_dOmega
// Is actually the same calculation as in func_fv...
// Since omega and Omega are additive...?
Vector3D rold = new Vector3D(0, 0, 0);
Vector3D vold = new Vector3D(0, 0, 0);
Vector3D omegaold = new Vector3D(0, 0, 0);
Quaternion qold = new Quaternion();
extract_r_q_v_omega(xv, rold, qold, vold, omegaold); // overkill but easy
// Fill in dqnew_by_domega = d(q x qwt)_by_dqwt . dqwt_by_domega
// Temp44A is d(q x qwt) by dqwt
MatrixFixed Temp44A = MatrixFixed.dq3_by_dq1(qold);
// Use function below for dqwt_by_domega
MatrixFixed Temp43A = new MatrixFixed(4, 3);
dqomegadt_by_domega(omegaold, delta_t, Temp43A);
// Multiply them together
MatrixFixed Temp43B = Temp44A * Temp43A;
// And then plug into Jacobian
dxnew_by_dn.Update(Temp43B, 3, 3);
// Finally do Q = dxnew_by_dn . Pnn . dxnew_by_dnT
QxRES.Update(dxnew_by_dn * Pnn * dxnew_by_dn.Transpose());
// cout << "QxRES" << QxRES;
}
/// <summary>
/// Calculate commonly used Jacobian part \partial q(\omega * \Delta t) / \partial \omega .
/// </summary>
/// <param name="omega"></param>
/// <param name="delta_t"></param>
/// <param name="dqomegadt_by_domega"></param>
public void dqomegadt_by_domega(Vector3D omega, float delta_t, MatrixFixed dqomegadt_by_domega)
{
// Modulus
float omegamod = (float)Math.Sqrt(omega.GetX() * omega.GetX() +
omega.GetY() * omega.GetY() +
omega.GetZ() * omega.GetZ());
// Use generic ancillary functions to calculate components of Jacobian
dqomegadt_by_domega.Put(0, 0, dq0_by_domegaA(omega.GetX(), omegamod, delta_t));
dqomegadt_by_domega.Put(0, 1, dq0_by_domegaA(omega.GetY(), omegamod, delta_t));
dqomegadt_by_domega.Put(0, 2, dq0_by_domegaA(omega.GetZ(), omegamod, delta_t));
dqomegadt_by_domega.Put(1, 0, dqA_by_domegaA(omega.GetX(), omegamod, delta_t));
dqomegadt_by_domega.Put(1, 1, dqA_by_domegaB(omega.GetX(), omega.GetY(), omegamod, delta_t));
dqomegadt_by_domega.Put(1, 2, dqA_by_domegaB(omega.GetX(), omega.GetZ(), omegamod, delta_t));
dqomegadt_by_domega.Put(2, 0, dqA_by_domegaB(omega.GetY(), omega.GetX(), omegamod, delta_t));
dqomegadt_by_domega.Put(2, 1, dqA_by_domegaA(omega.GetY(), omegamod, delta_t));
dqomegadt_by_domega.Put(2, 2, dqA_by_domegaB(omega.GetY(), omega.GetZ(), omegamod, delta_t));
dqomegadt_by_domega.Put(3, 0, dqA_by_domegaB(omega.GetZ(), omega.GetX(), omegamod, delta_t));
dqomegadt_by_domega.Put(3, 1, dqA_by_domegaB(omega.GetZ(), omega.GetY(), omegamod, delta_t));
dqomegadt_by_domega.Put(3, 2, dqA_by_domegaA(omega.GetZ(), omegamod, delta_t));
}
/// <summary>
/// Form the covariance matrix Q of the process noise associated with x_v .
/// </summary>
/// <param name="xv"></param>
/// <param name="v"></param>
/// <param name="delta_t"></param>
public override void func_Q(Vector xv, Vector v, float delta_t)
{
// Fill noise covariance matrix Pnn: this is the covariance of
// the noise vector (V)
// (Omega)
// that gets added to the state.
// Form of this could change later, but for now assume that
// V and Omega are independent, and that each of their components is
// independent...
float linear_velocity_noise_variance =
SD_A_component_filter * SD_A_component_filter * delta_t * delta_t;
float angular_velocity_noise_variance =
SD_alpha_component_filter * SD_alpha_component_filter * delta_t * delta_t;
// Independence means that the matrix is diagonal
MatrixFixed Pnn = new MatrixFixed(6, 6);
Pnn.Fill(0.0f);
Pnn.Put(0, 0, linear_velocity_noise_variance);
Pnn.Put(1, 1, linear_velocity_noise_variance);
Pnn.Put(2, 2, linear_velocity_noise_variance);
Pnn.Put(3, 3, angular_velocity_noise_variance);
Pnn.Put(4, 4, angular_velocity_noise_variance);
Pnn.Put(5, 5, angular_velocity_noise_variance);
// Form Jacobian dxnew_by_dn
// Is like this:
// I * delta_t 0
// 0 dqnew_by_dOmega
// Start by zeroing
MatrixFixed dxnew_by_dn = new MatrixFixed(7, 6);
dxnew_by_dn.Fill(0.0f);
// The translation part is just I \Delta t
MatrixFixed Temp33A = new MatrixFixed(3, 3);
Temp33A.SetIdentity();
Temp33A *= delta_t;
dxnew_by_dn.Update(Temp33A, 0, 0);
// qnew = q x \Omega \Deltat
// dqnew_by_d\Omega = dqnew_by_d\Omega\Delta t . d\Omega\Delta t_by_d\Omega
// Get the first part
Vector qRXYZ = xv.Extract(4, 3);
Quaternion qold = new Quaternion();
qold.SetRXYZ(qRXYZ);
MatrixFixed Temp44A = MatrixFixed.dq3_by_dq1(qold);
// Use function below for dqwt_by_dOmega
Vector Omega = new Vector(3);
Omega.Fill(SD_alpha_component_filter);
MatrixFixed Temp43A = new MatrixFixed(4, 3);
dqomegadt_by_domega(new Vector3D(Omega), delta_t, Temp43A);
// Multiply them together
MatrixFixed Temp43B = Temp44A * Temp43A;
// And then plug into Jacobian
dxnew_by_dn.Update(Temp43B, 3, 3);
// Finally do Q = dxnew_by_dn . Pnn . dxnew_by_dnT
QxRES.Update(dxnew_by_dn * Pnn * dxnew_by_dn.Transpose());
}