// Set the current innovation covariance S_i^{-1} for this
// particle. Called from
// Scene_Single::predict_partially_initialised_feature_measurements()
public void set_S(MatrixFixed Si)
{
Cholesky local_Si_cholesky = new Cholesky(Si);
m_SInv.Update(local_Si_cholesky.Inverse());
m_detS = SceneLib.Determinant(Si);
}
/// <summary>
/// Fast version of predict filter
/// </summary>
/// <param name="scene"></param>
/// <param name="u">Control vector of accelerations</param>
/// <param name="delta_t">time step in seconds</param>
public void predict_filter_fast(Scene_Single scene, Vector u, float delta_t)
{
Vector xv = scene.get_xv();
// Make model calculations: results are stored in RES matrices
scene.get_motion_model().func_fv_and_dfv_by_dxv(xv, u, delta_t);
scene.get_motion_model().func_Q(xv, u, delta_t);
Vector fvRES = scene.get_motion_model().get_fvRES();
xv.Update(fvRES);
Motion_Model mm = scene.get_motion_model();
MatrixFixed Pxx = scene.get_Pxx();
MatrixFixed dfv_by_dxvRES = mm.get_dfv_by_dxvRES();
MatrixFixed dfv_by_dxvRES_transposed = dfv_by_dxvRES.Transpose();
MatrixFixed QxRES = mm.get_QxRES();
Pxx.Update((dfv_by_dxvRES * Pxx * dfv_by_dxvRES_transposed) + QxRES);
// Change the covariances between vehicle state and feature states
foreach (Feature it in scene.feature_list)
{
MatrixFixed Pxy = it.get_Pxy();
Pxy.Update(dfv_by_dxvRES * Pxy);
}
}
/// <summary>
/// Solve the matrix equation M X = B, returning X.
/// </summary>
/// <param name="B"></param>
/// <returns></returns>
public MatrixFixed Solve(MatrixFixed B)
{
MatrixFixed x; // solution matrix
if (U_.Rows < U_.Columns)
{
// augment y with extra rows of
MatrixFixed yy = new MatrixFixed(U_.Rows, B.Columns); // zeros, so that it matches
yy.Fill(0);
yy.Update(B); // cols of u.transpose. ???
x = U_.ConjugateTranspose() * yy;
}
else
{
x = U_.ConjugateTranspose() * B;
}
int i, j;
for (i = 0; i < x.Rows; i++)
{ // multiply with diagonal 1/W
float weight = W_[i, i];
if (weight != 0) //vnl_numeric_traits<T>::zero)
{
weight = 1.0f / weight;
}
for (j = 0; j < x.Columns; j++)
{
x[i, j] *= weight;
}
}
x = V_ * x; // premultiply with v.
return(x);
}
/// <summary>
/// Simple overall prediction
/// </summary>
/// <param name="scene"></param>
/// <param name="u"></param>
/// <param name="delta_t"></param>
public void predict_filter_slow(Scene_Single scene, Vector u, float delta_t)
{
Debug.WriteLine("*** SLOW PREDICTION ***");
// What we need to do for the prediction:
// Calculate f and grad_f_x
// Calculate Q
// Form x(k+1|k) and P(k+1|k)
int size = (int)scene.get_total_state_size();
// First form original total state and covariance
Vector x = new Vector(size);
MatrixFixed P = new MatrixFixed(size, size);
scene.construct_total_state_and_covariance(ref x, ref P);
// Make model calculations: store results in RES matrices
Vector xv = scene.get_xv();
//Vector xv = new Vector(scene.get_xv());
scene.get_motion_model().func_fv_and_dfv_by_dxv(xv, u, delta_t);
scene.get_motion_model().func_Q(scene.get_xv(), u, delta_t);
// Find new state f
Vector f = new Vector(size);
// Feature elements of f are the same as x
f.Update(x);
f.Update(scene.get_motion_model().get_fvRES(), 0);
// Find new P
// Since most elements of df_by_dx are zero...
MatrixFixed df_by_dx = new MatrixFixed(size, size);
df_by_dx.Fill(0.0f);
// Fill the rest of the elements of df_by_dx: 1 on diagonal for features
for (int i = (int)scene.get_motion_model().STATE_SIZE; i < df_by_dx.Rows; i++)
{
df_by_dx[i, i] = 1.0f;
}
df_by_dx.Update(scene.get_motion_model().get_dfv_by_dxvRES(), 0, 0);
// Calculate the process noise
MatrixFixed Q = new MatrixFixed(size, size);
Q.Fill(0.0f);
Q.Update(scene.get_motion_model().get_QxRES(), 0, 0);
P.Update(df_by_dx * P * df_by_dx.Transpose());
P += Q;
scene.fill_state_and_covariance(f, P);
}
public void update_filter_internal_measurement_slow(Scene_Single scene, uint i)
{
// Size of measurement vector
uint size = ((Internal_Measurement)(scene.internal_measurement_vector[(int)i])).get_internal_measurement_model().MEASUREMENT_SIZE;
uint size2 = scene.get_total_state_size(); // Size of state vector
Vector x = new Vector(size2);
MatrixFixed P = new MatrixFixed(size2, size2);
scene.construct_total_state_and_covariance(ref x, ref P);
// cout << "x:" << x;
// cout << "P:" << P;
// 1. Form nu and dh_by_dx
Vector nu_tot = new Vector(size);
MatrixFixed dh_by_dx_tot = new MatrixFixed(size, size2);
MatrixFixed R_tot = new MatrixFixed(size, size);
scene.construct_total_internal_measurement_stuff(nu_tot, dh_by_dx_tot, R_tot, i);
// 2. Calculate S(k+1)
MatrixFixed S = new MatrixFixed(size, size);
//MatrixFixed Tempss2 = new MatrixFixed(size, size2);
//MatrixFixed Temps2s = new MatrixFixed(size2, size);
MatrixFixed dh_by_dx_totT = dh_by_dx_tot.Transpose();
S.Update(dh_by_dx_tot * P * dh_by_dx_totT);
S += R_tot;
// cout << "R_tot:" << R_tot;
// cout << "S:" << S;
// cout << "dh_by_dx_tot:" << dh_by_dx_tot;
// cout << "dh_by_dx_totT:" << dh_by_dx_totT;
// 3. Calculate W(k+1)
Cholesky S_cholesky = new Cholesky(S);
MatrixFixed W = P * dh_by_dx_totT * S_cholesky.Inverse();
// cout << "W:" << W;
// 4. Calculate x(k+1|k+1)
x += W * nu_tot;
// 5. Calculate P(k+1|k+1)
P -= W * S * W.Transpose();
scene.fill_state_and_covariance(x, P);
// cout << "x after update:" << x;
// cout << "P after update:" << P;
}
public void construct_total_internal_measurement_stuff(
Vector nu_tot, MatrixFixed dh_by_dx_tot,
MatrixFixed R_tot, uint total_state_size)
{
//uint size = internal_measurement_model.MEASUREMENT_SIZE;
//assert (nu_tot.Size() == size &&
//dh_by_dx_tot.Rows() == size &&
//dh_by_dx_tot.Cols() == total_state_size &&
//R_tot.Rows() == size && R_tot.Cols() == size);
//assert(successful_internal_measurement_flag);
nu_tot.Fill(0.0f);
dh_by_dx_tot.Fill(0.0f);
R_tot.Fill(0.0f);
nu_tot.Update(nuv, 0);
dh_by_dx_tot.Update(dhv_by_dxv, 0, 0);
R_tot.Update(Rv, 0, 0);
}
/// <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>
/// 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());
}
