/// <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> /// 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 override void func_zeroedyigraphics_and_Pzeroedyigraphics(Vector yi, Vector xv, MatrixFixed Pxx, MatrixFixed Pxyi, MatrixFixed Pyiyi) { ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.func_xp(xv); // In this case (where the feature state is the same as the graphics // state) zeroedyigraphics is the same as zeroedyi func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(yi, ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.get_xpRES()); zeroedyigraphicsRES.Update(zeroedyiRES); MatrixFixed dzeroedyigraphics_by_dxv = dzeroedyi_by_dxpRES * ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model.get_dxp_by_dxvRES(); PzeroedyigraphicsRES.Update(dzeroedyigraphics_by_dxv * Pxx * dzeroedyigraphics_by_dxv.Transpose() + dzeroedyi_by_dyiRES * Pxyi.Transpose() * dzeroedyigraphics_by_dxv.Transpose() + dzeroedyigraphics_by_dxv * Pxyi * dzeroedyi_by_dyiRES.Transpose() + dzeroedyi_by_dyiRES * Pyiyi * dzeroedyi_by_dyiRES.Transpose()); }
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; }
/// <summary> /// Update the filter in a simple overall way /// </summary> /// <param name="scene"></param> public void total_update_filter_slow(Scene_Single scene) { // Steps to update the total filter: // 1. Form h and dh_by_dx and R(k+1) and z // 2. Calculate S(k+1) // 3. Calculate W(k+1) // 4. Calculate x(k+1|k+1) // 5. Calculate P(k+1|k+1) uint size = scene.get_successful_measurement_vector_size(); // Size of measurement vector 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); // 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_measurement_stuff(nu_tot, dh_by_dx_tot, R_tot); // 2. Calculate S(k+1) MatrixFixed temp_matrix = P * dh_by_dx_tot.Transpose(); //pre-calculate to speed up subsequent stuff MatrixFixed S = dh_by_dx_tot * temp_matrix; S += R_tot; // 3. Calculate W(k+1) Cholesky S_cholesky = new Cholesky(S); MatrixFixed W = temp_matrix * S_cholesky.Inverse(); // 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); }
/// <summary> /// Constructor for partially-initialised features. /// </summary> /// <param name="id">reference to the feature</param> /// <param name="lab"></param> /// <param name="list_pos"></param> /// <param name="scene"></param> /// <param name="h"></param> /// <param name="p_i_f_m_m"></param> public Feature(byte[] id, uint lab, uint list_pos, Scene_Single scene, Vector h, Partially_Initialised_Feature_Measurement_Model p_i_f_m_m, Vector feature_colour) { feature_measurement_model = p_i_f_m_m; partially_initialised_feature_measurement_model = p_i_f_m_m; fully_initialised_feature_measurement_model = null; // Stuff below substituted from Feature_common // Feature_common(id, lab, list_pos, scene, h); feature_constructor_bookeeping(); identifier = id; label = lab; position_in_list = list_pos; // Position of new feature in list position_in_total_state_vector = 0; // This should be set properly colour = feature_colour; //when feature is added // Save the vehicle position where this feature was acquired scene.get_motion_model().func_xp(scene.get_xv()); //xp_orig = scene.get_motion_model().get_xpRES(); xp_orig = new Vector(scene.get_motion_model().get_xpRES()); // Call model functions to calculate feature state, measurement noise // and associated Jacobians. Results are stored in RES matrices // First calculate "position state" and Jacobian scene.get_motion_model().func_xp(scene.get_xv()); scene.get_motion_model().func_dxp_by_dxv(scene.get_xv()); // Now ask the model to initialise the state vector and calculate Jacobians // so that I can go and calculate the covariance matrices partially_initialised_feature_measurement_model.func_ypi_and_dypi_by_dxp_and_dypi_by_dhi_and_Ri(h, scene.get_motion_model().get_xpRES()); // State y //y = partially_initialised_feature_measurement_model.get_ypiRES(); y = new Vector(partially_initialised_feature_measurement_model.get_ypiRES()); // Temp_FS1 will store dypi_by_dxv MatrixFixed Temp_FS1 = partially_initialised_feature_measurement_model.get_dypi_by_dxpRES() * scene.get_motion_model().get_dxp_by_dxvRES(); // Pxy Pxy = scene.get_Pxx() * Temp_FS1.Transpose(); // Pyy Pyy = Temp_FS1 * scene.get_Pxx() * Temp_FS1.Transpose() + partially_initialised_feature_measurement_model.get_dypi_by_dhiRES() * partially_initialised_feature_measurement_model.get_RiRES() * partially_initialised_feature_measurement_model.get_dypi_by_dhiRES().Transpose(); // Covariances of this feature with others int j = 0; foreach (Feature it in scene.get_feature_list_noconst()) { if (j < position_in_list) { // new Pypiyj = dypi_by_dxv . Pxyj // Size of this is FEATURE_STATE_SIZE(new) by FEATURE_STATE_SIZE(old) MatrixFixed m = it.get_Pxy(); MatrixFixed newPyjypi_to_store = (Temp_FS1 * m).Transpose(); //add to the list matrix_block_list.Add(newPyjypi_to_store); } j++; } known_feature_label = -1; }
/// <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()); }