/// <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);
}
/// <summary>
/// Constructor for known features. The different number of
/// arguments differentiates it from the constructor for partially-initialised
/// features
/// </summary>
/// <param name="id">reference to the feature identifier</param>
/// <param name="?"></param>
public Feature(classimage_mono id, uint lab, uint list_pos,
Scene_Single scene, Vector y_known,
Vector xp_o,
Feature_Measurement_Model f_m_m, uint k_f_l)
{
feature_measurement_model = f_m_m;
feature_constructor_bookeeping();
identifier = id;
label = lab;
position_in_list = list_pos; // Position of new feature in list
// Save the vehicle position where this feature was acquired
xp_orig = new Vector(xp_o);
// Straighforward initialisation of state and covariances
y = y_known;
Pxy = new MatrixFixed(scene.get_motion_model().STATE_SIZE, feature_measurement_model.FEATURE_STATE_SIZE);
Pxy.Fill(0.0f);
Pyy = new MatrixFixed(feature_measurement_model.FEATURE_STATE_SIZE, feature_measurement_model.FEATURE_STATE_SIZE);
Pyy.Fill(0.0f);
int i = 0;
MatrixFixed newPyjyi_to_store;
foreach (Feature it in scene.get_feature_list_noconst())
{
if (i < position_in_list)
{
newPyjyi_to_store = new MatrixFixed(
it.get_feature_measurement_model().FEATURE_STATE_SIZE,
feature_measurement_model.FEATURE_STATE_SIZE);
//add to the list
matrix_block_list.Add(newPyjyi_to_store);
}
i++;
}
known_feature_label = (int)k_f_l;
if (feature_measurement_model.fully_initialised_flag)
{
partially_initialised_feature_measurement_model = null;
fully_initialised_feature_measurement_model =
(Fully_Initialised_Feature_Measurement_Model)feature_measurement_model;
}
else
{
fully_initialised_feature_measurement_model = null;
partially_initialised_feature_measurement_model =
(Partially_Initialised_Feature_Measurement_Model)feature_measurement_model;
}
}
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);
}
public void construct_total_measurement_stuff(Vector nu_tot,
MatrixFixed dh_by_dx_tot,
MatrixFixed R_tot)
{
uint size = successful_measurement_vector_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);
nu_tot.Fill(0.0f);
dh_by_dx_tot.Fill(0.0f);
R_tot.Fill(0.0f);
uint vector_position = 0;
Feature it;
for (int i=0;i<selected_feature_list.Count;i++)
{
it = (Feature)selected_feature_list[i];
if (it.get_successful_measurement_flag())
{
nu_tot.Update(it.get_nu(), (int)vector_position);
dh_by_dx_tot.Update(it.get_dh_by_dxv(), (int)vector_position, 0);
dh_by_dx_tot.Update(it.get_dh_by_dy(), (int)vector_position,
(int)it.get_position_in_total_state_vector());
R_tot.Update(it.get_R(), (int)vector_position, (int)vector_position);
vector_position += it.get_feature_measurement_model().MEASUREMENT_SIZE;
}
}
}
//*************************Some important operations***************************
public void zero_axes()
{
uint size = get_total_state_size();
Vector x = new Vector(size);
x.Fill(0.0f);
MatrixFixed dxnew_by_dxold = new MatrixFixed(size, size);
dxnew_by_dxold.Fill(0.0f);
// We form the new state and Jacobian of the new state w.r.t. the old state
uint state_position = 0;
// Robot part
motion_model.func_zeroedxv_and_dzeroedxv_by_dxv(xv);
x.Update(motion_model.get_zeroedxvRES(), 0);
dxnew_by_dxold.Update(motion_model.get_dzeroedxv_by_dxvRES(), 0, 0);
state_position += motion_model.STATE_SIZE;
// Each feature in turn
uint feature_no = 0;
// Copy these to be on the safe side
motion_model.func_xp(xv);
Vector local_xp = new Vector(motion_model.get_xpRES());
//Vector local_xp = motion_model.get_xpRES();
motion_model.func_dxp_by_dxv(xv);
//MatrixFixed local_dxp_by_dxv = motion_model.get_dxp_by_dxvRES();
MatrixFixed local_dxp_by_dxv = new MatrixFixed(motion_model.get_dxp_by_dxvRES());
Feature it;
MatrixFixed Temp_FS1;
for (int i=0; i < feature_list.Count; i++)
{
it = (Feature)feature_list[i];
it.get_feature_measurement_model().func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(it.get_y(), local_xp);
x.Update(it.get_feature_measurement_model().get_zeroedyiRES(), (int)state_position);
// Calculate dzeroedyi_by_dxv in Temp_FS1
Temp_FS1 = it.get_feature_measurement_model().get_dzeroedyi_by_dxpRES() * local_dxp_by_dxv;
dxnew_by_dxold.Update(Temp_FS1, (int)state_position, 0);
dxnew_by_dxold.Update(it.get_feature_measurement_model().get_dzeroedyi_by_dyiRES(),
(int)state_position, (int)state_position);
// We also want to redefine xp_orig for this feature: the robot
// position at which is was initialised into the map (used by
// functions for checking visibility)
motion_model.func_xpredef_and_dxpredef_by_dxp_and_dxpredef_by_dxpdef(
it.get_xp_orig(), local_xp);
it.set_xp_orig(it.get_feature_measurement_model().get_motion_model().get_xpredefRES());
feature_no++;
state_position += it.get_feature_measurement_model().FEATURE_STATE_SIZE;
}
// Check we've counted properly
//assert (feature_no == feature_list.size() &&
// state_position == total_state_size);
// Do a Jacobian transform to get the new covariance
MatrixFixed P = new MatrixFixed(size, size);
construct_total_covariance(ref P);
P = dxnew_by_dxold * P * dxnew_by_dxold.Transpose();
// Finally load the scene data structures with the new state and covariance
fill_states(x);
fill_covariances(P);
}
/// <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>
/// Calculate inverse of transpose.
/// </summary>
/// <returns></returns>
public MatrixFixed TransposeInverse()
{
MatrixFixed Winverse = new MatrixFixed(Winverse_.Rows,Winverse_.Columns);
Winverse.Fill(0);
for (int i=0;i<rank_;i++)
Winverse[i,i]=Winverse_[i,i];
return (U_ * Winverse * V_.ConjugateTranspose());
}
/// <summary>
/// Calculate pseudo-inverse.
/// </summary>
/// <param name="rank"></param>
/// <returns></returns>
public MatrixFixed PseudoInverse(int rank)
{
MatrixFixed Winverse = new MatrixFixed(Winverse_.Rows,Winverse_.Columns);
Winverse.Fill(0);
for (int i=0;i<rank;i++)
Winverse[i,i]=Winverse_[i,i];
return (V_ * Winverse * U_.ConjugateTranspose());
}
/// <summary>
/// Recompose SVD to U*W*V'.
/// </summary>
/// <returns></returns>
public MatrixFixed Recompose()
{
MatrixFixed W = new MatrixFixed(W_.Rows,W_.Columns);
W.Fill(0);
for (int i=0;i<rank_;i++)
W[i,i]=W_[i,i];
return (U_*W*V_.ConjugateTranspose());
}
/// <summary>
/// Calculate the mean and covariance of \lambda over all the particles,
/// i.e.
///
/// \text{mean} &= \mu = \sum_i \lambda_i p(i) \nonumber \\
/// \text{mean} &= \sum_i \lambda_i\lambda_i^T p(i) - \mu\mu^T \nonumber
///
/// The result is not returned, but is instead stored in the class to be read using
/// get_mean() and get_covariance().
/// </summary>
public void calculate_mean_and_covariance()
{
// Vector which will store expected value of lambda * lambda^T
// (allows us to do this calculation with one pass through particles)
MatrixFixed ExpectedSquared = new MatrixFixed(PARTICLE_DIMENSION, PARTICLE_DIMENSION);
ExpectedSquared.Fill(0.0f);
// Zero mean vector before filling it up
mean.Fill(0.0f);
foreach (Particle it in particle_vector)
{
mean += it.lambda * it.probability;
ExpectedSquared += it.probability * Vector.OuterProduct(it.lambda, it.lambda);
}
covariance = ExpectedSquared - Vector.OuterProduct(mean, mean);
}
/// <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>
/// Read the initial state vector and covariance from the settings class.
/// Since state \f$ x_v \f$ and covariance \f$ P_{xx} \f$ are not stored in
/// the class, these are passed by reference to be filled in by this function.
/// </summary>
/// <param name="settings"></param>
/// <param name="initial_xv"></param>
/// <param name="initial_Pxx"></param>
public void read_initial_state(Settings settings,
ref Vector initial_xv,
ref MatrixFixed initial_Pxx)
{
ArrayList values;
// Check that the motion model is correct
values = settings.get_entry("InitialState", "MotionModel");
if ((String)values[0] != motion_model_type)
{
Debug.WriteLine("Attempted to read an initial state with a motion model of type " +
motion_model_type + " where the initialisation data in the [InitialState] section" +
" reports the type " + settings.get_entry("InitialState", "MotionModel") + ".");
//throw Scene::InitialisationError(error.str());
}
// Make sure the vector and matrix are the correct sizes
initial_xv = new Vector(STATE_SIZE);
initial_Pxx = new MatrixFixed(STATE_SIZE, STATE_SIZE);
//initial_xv.Resize(STATE_SIZE);
//initial_Pxx.Resize(STATE_SIZE, STATE_SIZE);
initial_xv.Fill(0.0f);
initial_Pxx.Fill(0.0f);
values = settings.get_entry("InitialState", "xv");
String xv_stream = (String)values[0];
initial_xv.ReadASCII(xv_stream);
values = settings.get_entry("InitialState", "Pxx");
String Pxx_stream = (String)values[0];
initial_Pxx.ReadASCII(Pxx_stream);
}
/// <summary>
/// Read the initial state vector and covariance from a stream.
/// Since state \f$ x_v \f$ and covariance \f$ P_{xx} \f$ are not stored in
/// the class, these are passed by reference to be filled in by this function.
/// </summary>
/// <param name="stream"></param>
/// <param name="initial_xv"></param>
/// <param name="initial_Pxx"></param>
public void read_initial_state(BinaryReader stream,
ref Vector initial_xv,
ref MatrixFixed initial_Pxx)
{
// Make sure the vector and matrix are the correct sizes
initial_xv.Resize(STATE_SIZE);
initial_Pxx.Resize(STATE_SIZE, STATE_SIZE);
initial_xv.Fill(0.0f);
initial_Pxx.Fill(0.0f);
for (int r = 0; r < STATE_SIZE; r++)
initial_xv[r] = stream.ReadSingle();
for (int r = 0; r < STATE_SIZE; r++)
for (int c = 0; c < STATE_SIZE; c++)
initial_Pxx[r, c] = stream.ReadSingle();
}
/// <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());
}
