C# Vector.Extract方法代码示例

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

        }