Пример #1
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);
        }
Пример #2
0
        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;
        }
Пример #3
0
        /// <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);
        }