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()); }