Example #1
0
        /// <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);
        }
Example #2
0
        /// <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);
        }
Example #3
0
        /// <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(byte[] 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;
            }
        }
Example #4
0
        /// <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());
        }
Example #5
0
        /// <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());
        }
Example #6
0
        /// <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());
        }
Example #7
0
        /// <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);
        }
        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);
        }
Example #9
0
        /// <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;
        }
Example #10
0
        /// <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());
        }