Example #1
0
        // Set the current innovation covariance S_i^{-1} for this
        // particle. Called from
        // Scene_Single::predict_partially_initialised_feature_measurements()
        public void set_S(MatrixFixed Si)
        {
            Cholesky local_Si_cholesky = new Cholesky(Si);

            m_SInv.Update(local_Si_cholesky.Inverse());
            m_detS = SceneLib.Determinant(Si);
        }
Example #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;
        }
Example #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);
        }
Example #4
0
        /// <summary>
        /// Make a measurement of a feature. This function calls elliptical_search() to
        /// find the best match within three standard deviations of the predicted location.
        /// </summary>
        /// <param name="patch">The identifier for this feature (in this case an image patch)</param>
        /// <param name="z">The best image location match for the feature, to be filled in by this function</param>
        /// <param name="h">The expected image location</param>
        /// <param name="S">The expected location covariance, used to specify the search region.</param>
        /// <returns></returns>
        public override bool measure_feature(byte[] patch,
		                                     int patchwidth,
		                                     ref Vector z, 
		                                     Vector vz, 
		                                     Vector h, 
		                                     MatrixFixed S, 
		                                     Random rnd)
        {
            Cholesky S_cholesky = new Cholesky(S);
            MatrixFixed Sinv = S_cholesky.Inverse();

            uint u_found = 0, v_found = 0;
            
            if (SceneLib.elliptical_search(image, image_width, image_height,
			                               patch, patchwidth, patchwidth,
                                           h, Sinv, 
			                               ref u_found, 
			                               ref v_found, 
			                               vz, 
			                               Camera_Constants.BOXSIZE, 
			                               outputimage,
			                               outputimage_width, outputimage_height,
			                               show_ellipses, 
			                               calibrating, 
			                               rnd) != true)
            {
                // Feature not successfully matched
                return false;
            }

            z.Put(0, (float)u_found);
            z.Put(1, (float)v_found);
            
            return true;
        }
Example #5
0
 // Set the current innovation covariance S_i^{-1} for this
 // particle. Called from 
 // Scene_Single::predict_partially_initialised_feature_measurements() 
 public void set_S(MatrixFixed Si)
 {
     Cholesky local_Si_cholesky = new Cholesky(Si);
     m_SInv.Update(local_Si_cholesky.Inverse());
     m_detS = SceneLib.Determinant(Si);
 }
Example #6
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;
        }
Example #7
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);

        }