C# MatrixFixed.SetIdentity方法代码示例

        /// <summary>
        /// Calculate the image position measurement noise at this location.
        /// </summary>
        /// <param name="h">The image location.  This is not constant across the image. It has the value of m_measurement_sd at the centre, increasing with radial distance to 2*m_measurement_sd at the corners</param>
        /// <returns></returns>
        public virtual MatrixFixed MeasurementNoise(Vector h)
        {
            // Distance of point we are considering from image centre
            float distance = (h - m_centre).Magnitude();
            float max_distance = m_centre.Magnitude();
            float ratio = distance / max_distance; // goes from 0 to 1

            float SD_image_filter_to_use = m_measurement_sd * (1.0f + ratio);

            float measurement_noise_variance = SD_image_filter_to_use * SD_image_filter_to_use;
  
            // RiRES is diagonal
            MatrixFixed noise = new MatrixFixed(2,2);
            noise.SetIdentity(measurement_noise_variance);

            return noise;
        }

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

        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="xv">position and orientation of the camera</param>
        /// <param name="u">Control vector of accelerations</param>
        /// <param name="delta_t">time step in seconds</param>
        public override void func_fv_and_dfv_by_dxv(Vector xv, Vector u, float delta_t)
        {
            Vector3D rold, vold, omegaold, rnew, vnew, omeganew;
            Quaternion qold, qnew;            

            // Separate things out to make it clearer
            rold = new Vector3D(0, 0, 0);
            vold = new Vector3D(0, 0, 0);
            omegaold = new Vector3D(0, 0, 0);
            qold = new Quaternion();
            extract_r_q_v_omega(xv, rold, qold, vold, omegaold);

            Vector3D acceleration = new Vector3D(u);
           
            // rnew = r + v * delta_t
            //rnew = (Vector3D)((Point3D)rold + (Point3D)vold * delta_t);
            rnew = new Vector3D(rold + vold * delta_t);

            // qnew = q x q(omega * delta_t)

            // Keep qwt ( = q(omega * delta_t)) for later use
            Quaternion qwt = new Quaternion(omegaold * delta_t);

            qnew = qold.Multiply(qwt);

            // vnew = v
            vnew = new Vector3D(vold + acceleration * delta_t);
  
            // omeganew = omega
            omeganew = omegaold;

            

            // Put it all together
            compose_xv(ref rnew, ref qnew, ref vnew, ref omeganew, ref fvRES);

            // cout << "rold qold vold omegaold" << rold << qold 
            //      << vold << omegaold;
            // cout << "rnew qnew vnew omeganew" << rnew << qnew 
            //      << vnew << omeganew;

            // Now on to the Jacobian...
            // Identity is a good place to start since overall structure is like this
            // I       0             I * delta_t   0
            // 0       dqnew_by_dq   0             dqnew_by_domega
            // 0       0             I             0
            // 0       0             0             I
            dfv_by_dxvRES.SetIdentity();

            // Fill in dxnew_by_dv = I * delta_t
            MatrixFixed Temp33A = new MatrixFixed(3,3);
            Temp33A.SetIdentity();
            Temp33A *= delta_t;
            dfv_by_dxvRES.Update(Temp33A, 0, 7);

            // Fill in dqnew_by_dq
            // qnew = qold x qwt  ( = q3 = q2 x q1 in Scene/newbits.cc language)
            MatrixFixed Temp44A = MatrixFixed.dq3_by_dq2(qwt); //4,4
            dfv_by_dxvRES.Update(Temp44A, 3, 3);

            // Fill in dqnew_by_domega = d(q x qwt)_by_dqwt . dqwt_by_domega
            Temp44A = MatrixFixed.dq3_by_dq1(qold); // Temp44A is d(q x qwt) by dqwt
 
            // 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 plug it in
            dfv_by_dxvRES.Update(Temp43B, 3, 10);
             
             

            // cout << "dfv_by_dxvRES" << dfv_by_dxvRES;
        }

        /// <summary>
        /// Unpack and return unitary part Q.
        /// </summary>
        /// <returns></returns>
        public MatrixFixed Q()
        {
            int m = qrdc_out_.Columns; // column-major storage
            int n = qrdc_out_.Rows;

            bool verbose = false;

            if (Q_ == null) 
            {
                Q_ = new MatrixFixed(m,m);
                // extract Q.
                if (verbose) 
                {
                    Debug.WriteLine("__FILE__: VNL::QR<T>Q()");
                    Debug.WriteLine("m,n = " + Convert.ToString(m) + ", " + Convert.ToString(n));
                    Debug.WriteLine("qr0 = [" + qrdc_out_ + "];");
                    Debug.WriteLine("aux = [" + qraux_ + "];");
                }

                Q_.SetIdentity();
                MatrixFixed Q = Q_;

                Vector v = new Vector(m);
                v.Fill(0);
                Vector w = new Vector(m);
                w.Fill(0);

                // Golub and vanLoan, p199.  backward accumulation of householder matrices
                // Householder vector k is [zeros(1,k-1) qraux_[k] qrdc_out_[k,:]]
                for (int k = n-1; k >= 0; --k) 
                {
                    if (k >= m) continue;
                    // Make housevec v, and accumulate norm at the same time.
                    v[k] = qraux_[k];
                    float sq = (v[k] * v[k]);
                    for (int j = k+1; j < m; ++j) 
                    {
                        v[j] = qrdc_out_[k,j];
                        sq += (v[j] * v[j]);
                    }
                    //if (verbose) MatlabPrint(std::cerr, v, "v");

                    // Premultiply emerging Q by house(v), noting that v[0..k-1] == 0.
                    // Q_new = (1 - (2/v'*v) v v')Q
                    // or Q -= (2/v'*v) v (v'Q)
                    if (sq > 0.0) 
                    {
                        float scale = 2.0f/sq;
                        // w = (2/v'*v) v' Q
                        for (int i = k; i < m; ++i) 
                        {
                            w[i] = 0.0f;
                            for (int j = k; j < m; ++j)
                                w[i] += scale * v[j] * Q[j, i];
                                //w[i] += scale * ComplexTraits.Conjugate(v[j]) * Q[j, i];
                        }
                        //if (verbose) VNL::MatlabPrint(std::cerr, w, "w");

                        // Q -= v w
                        for (int i = k; i < m; ++i)
                            for (int j = k; j < m; ++j)
                                Q[i,j] -= (v[i]) * (w[j]);
                    }
                }
            }
            return Q_;
        }