public void func_ri(Vector ypi)
{
riRES.SetVNL3(ypi.Extract(3, 0));
}
public void func_hhati(Vector ypi)
{
hhatiRES.SetVNL3(ypi.Extract(3, 3));
}
public void fill_states(Vector V)
{
//assert (V.Size() == total_state_size);
uint y_position = 0;
xv.Update(V.Extract((int)motion_model.STATE_SIZE, (int)y_position));
y_position += motion_model.STATE_SIZE;
uint y_feature_no = 0;
Feature it;
for (int i=0;i<feature_list.Count;i++)
{
if (y_position < V.Size())
{
it = (Feature)feature_list[i];
it.set_y(V.Extract((int)it.get_feature_measurement_model().FEATURE_STATE_SIZE, (int)y_position));
y_feature_no++;
y_position += it.get_feature_measurement_model().FEATURE_STATE_SIZE;
}
}
//assert (y_feature_no <= feature_list.size() && y_position <= total_state_size);
}
/// <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>
/// Extract the position and orientation from the state vector.
/// (This is the first seven elements in this case.)
/// </summary>
/// <param name="xv"></param>
public override void func_xp(Vector xv)
{
xpRES = xv.Extract(7, 0);
}
/// <summary>
/// Extract the rotation (quaternion) part of the state position vector.
/// </summary>
/// <param name="?"></param>
public void func_q(Vector xp)
{
Vector qvec = xp.Extract(4, 3);
qRES.SetRXYZ(qvec);
}
/// <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());
}
