C# MatrixFixed.Transpose方法代码示例

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

        /// <summary>
        /// Step the MonoSLAM application on by one frame. This should be called every time
        /// a new frame is captured (and care should be taken to avoid skipping frames).
        /// Before calling this function, Scene_Single::load_new_image() should be called
        /// (e.g. using
        /// <code>monoslaminterface.GetRobotNoConst()->load_new_image()</code>), since the
        /// first parameter to GoOneStep() is currently ignored.
        /// 
        /// GoOneStep() performs the following processing steps:
        /// - Kalman filter prediction step (by calling Kalman::predict_filter_fast(),
        ///   with a zero control vector)
        /// - Select a set of features to make measurements from (set by
        ///   SetNumberOfFeaturesToSelect())
        /// - Predict the locations and and make measurements of those features
        /// - Kalman filter update step
        /// - Delete any bad features (those that have repeatedly failed to be matched)
        /// - If we are not currently initialising a enough new features, and the camera is
        ///   translating, and <code>currently_mapping_flag</code> is set, initialise a new
        ///   feature somewhere sensible
        /// - Update the partially-initialised features
        /// </summary>
        /// <param name="img">camera image</param>
        /// <param name="delta_t">The time between frames in seconds</param>
        /// <param name="currently_mapping_flag">Set to be true if new features should be detected and added to the map.</param>
        /// <returns></returns>
        public bool GoOneStep(float delta_t, bool currently_mapping_flag)
        {
            if (delta_t > 0)
            {
                // update the integral image for use in feature detection
                //img.updateIntegralImage();

                // nullify image selection
                robot.nullify_image_selection();

                init_feature_search_region_defined_flag = false;

                // Control vector of accelerations
                Vector u = new Vector(3);
                u.Fill(0.0f);

                sim_or_rob.set_control(u, delta_t);

                // Record the current position so that I can estimate velocity
                // (We can guarantee that the state vector has position; we can't 
                // guarantee that it has velocity.)
                Vector xv = scene.get_xv();
                scene.get_motion_model().func_xp(xv);
                Vector prev_xp_pos = (scene.get_motion_model().get_xpRES()).Extract(3);

                // Prediction step
                if (currently_mapping_flag)
                    kalman.predict_filter_fast(scene, u, delta_t);

                // if features are not seen the first time try a few more times
                int tries = 0;
                number_of_visible_features = 0;
                while (((tries == 0) || (scene.get_no_selected() < 2)) && (tries < 5))
                {
                    number_of_visible_features = scene.auto_select_n_features(NUMBER_OF_FEATURES_TO_SELECT);

                    if (scene.get_no_selected() > 0)
                    {
                        //scene.predict_measurements(); // commented out in original code                        

                        number_of_matched_features = (uint)SceneLib.make_measurements(scene, sim_or_rob, rnd);

                        if (scene.get_successful_measurement_vector_size() != 0)
                        {
                            // this function is the slowest part of the algorithm
                            kalman.total_update_filter_slow(scene);

                            scene.normalise_state();
                        }

                    }

                    tries++;
                }


                if (currently_mapping_flag)
                    scene.delete_bad_features();

                // Let's enforce symmetry of covariance matrix...
                // Add to transpose and divide by 2                
                uint tot_state_size = scene.get_total_state_size();
                MatrixFixed Pxx = new MatrixFixed(tot_state_size, tot_state_size);
                scene.construct_total_covariance(ref Pxx);
                MatrixFixed PxxT = Pxx.Transpose();                

                Pxx.Update(Pxx * 0.5f + PxxT * 0.5f);
                scene.fill_covariances(Pxx);
                
                // Look at camera speed estimate
                // Get the current position and estimate the speed from it
                xv = scene.get_xv();
                scene.get_motion_model().func_xp(xv);
                Vector xp_pos = scene.get_motion_model().get_xpRES().Extract(3);
                velocity = (xp_pos - prev_xp_pos) / delta_t;
//.........这里部分代码省略.........

        //*************************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>
        /// 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>
        /// 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;
        }

        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>
        /// Calculate the innovation covariance. This is the overall measurement
        /// uncertainty in the feature, a combination of the uncertainty in the robot
        /// state, the feature state, and the measurement uncertainty. This calculation is
        /// generic to all feature measurement models, but could be over-ridden if necessary
        /// (e.g. for an efficient implementation). The innovation covariance is given by
        /// \f[S_i = \frac{\partial h_i}{\partial x_v} P_{xx}
        /// \frac{\partial h_i}{\partial x_v}^T
        /// + \frac{\partial h_i}{\partial x_v} P_{xy_i}
        /// \frac{\partial h_i}{\partial y_i}^T
        /// + \frac{\partial h_i}{\partial y_i} P_{y_i x}
        /// \frac{\partial h_i}{\partial x_v}^T
        /// + \frac{\partial h_i}{\partial y_i} P_{y_i y_i}
        /// \frac{\partial h_i}{\partial y_i}^T
        /// + R_i ]
        /// where R_i is the noise covariance of measurements (usually assumed to
        /// be diagonal with magnitude determined by the image resolution).
        /// </summary>
        /// <param name="Pxx">The covariance P_{xy_i} between the robot state x_v and the feature state y_i .</param>
        /// <param name="Pxyi">The covariance of the feature, P_{y_iy_i} </param>
        /// <param name="Pyiyi">The Jacobian \frac{\partial h_i}{\partial x_v} between the feature measurement h_i and the robot state x_v </param>
        /// <param name="dhi_by_dxv">The Jacobian  \frac{\partial h_i}{\partial y_i} between the feature measurement h_i and the feature state y_i </param>
        /// <param name="dhi_by_dyi">The innovation covariance R_i</param>
        /// <param name="Ri"></param>
        public void func_Si(MatrixFixed Pxx, 
                            MatrixFixed Pxyi,
                            MatrixFixed Pyiyi,
                            MatrixFixed dhi_by_dxv,
                            MatrixFixed dhi_by_dyi,
                            MatrixFixed Ri)
        {
            // Zero SiRES and add bits on
            SiRES.Fill(0.0f);

            SiRES += dhi_by_dxv * Pxx * dhi_by_dxv.Transpose();
  
            MatrixFixed Temp_MM1 = dhi_by_dxv * Pxyi * dhi_by_dyi.Transpose();
            SiRES += Temp_MM1;

            SiRES += Temp_MM1.Transpose();

            SiRES += dhi_by_dyi * Pyiyi * dhi_by_dyi.Transpose();

            SiRES += Ri;
        }    

        public void func_Sv(MatrixFixed Pxx,
                            MatrixFixed dhv_by_dxv,
                            MatrixFixed Rv)
        {
            // Zero SiRES and add bits on
            SvRES.Fill(0.0f);

            SvRES += dhv_by_dxv * Pxx * dhv_by_dxv.Transpose();

            SvRES += Rv;
        }

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

        }