public override void func_xpose_and_Rpose(Vector xv)
{
func_xp(xv);
// Turn xp vector (x, y, z, q0, q1, q2, q3) into pose
xposeRES = xpRES.Extract(3, 0);
Vector qvec = xpRES.Extract(4, 3);
Quaternion q = new Quaternion();
q.SetRXYZ(qvec);
RposeRES = q.RotationMatrix();
}
/// <summary>
/// Extract the component parts of the state x_v . Fills matrices r, q, v, omega with values.
/// </summary>
/// <param name="xv"></param>
/// <param name="r"></param>
/// <param name="q"></param>
/// <param name="v"></param>
/// <param name="omega"></param>
public void extract_r_q_v_omega(Vector xv, Vector3D r, Quaternion q, Vector3D v, Vector3D omega)
{
r.SetVNL3(xv.Extract(3, 0));
Vector qRXYZ = xv.Extract(4, 3);
q.SetRXYZ(qRXYZ);
v.SetVNL3(xv.Extract(3, 7));
omega.SetVNL3(xv.Extract(3, 10));
}
/// <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());
}