C# MatrixFixed.Fill方法代码示例

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

        }