public MatrixFixed(MatrixFixed m)
 {
     init(m.Rows, m.Columns);
     for (int i = 0; i < Rows; i++)
         for (int j = 0; j < Columns; j++)
             m_matrix[i, j] = m[i, j];
 }

        /// <summary>
        /// Base-class constructor. This should be called in the constructor of any 
        /// derived class.
        /// </summary>
        /// <param name="position_state_size">The number of dimensions in the position state</param>
        /// <param name="state_size">The total state size</param>
        /// <param name="control_size">The control state size</param>
        /// <param name="m_m_d_t">A string representing the motion model dimensionality type </param>
        /// <param name="m_m_t">A unique string representing this particular motion model type</param>
        public Motion_Model(uint position_state_size, 
			   uint state_size, 
			   uint control_size, 
			   String m_m_d_t, String m_m_t)
        {
            POSITION_STATE_SIZE = position_state_size;
            STATE_SIZE = state_size;
            CONTROL_SIZE = control_size;
            motion_model_dimensionality_type = m_m_d_t;
            motion_model_type = m_m_t;

            fvRES = new Vector(STATE_SIZE);
            xpRES = new Vector(POSITION_STATE_SIZE);
            fv_noisyRES = new Vector(STATE_SIZE);
            xvredefRES = new Vector(STATE_SIZE);
            xvnormRES = new Vector(STATE_SIZE);
            zeroedxvRES = new Vector(STATE_SIZE);
            xpredefRES = new Vector(POSITION_STATE_SIZE);

            dfv_by_dxvRES = new MatrixFixed(STATE_SIZE, STATE_SIZE);
            QxRES = new MatrixFixed(STATE_SIZE, STATE_SIZE);
            dxp_by_dxvRES = new MatrixFixed(POSITION_STATE_SIZE, STATE_SIZE);           
            dxvredef_by_dxvRES = new MatrixFixed(STATE_SIZE, STATE_SIZE);
            dxvredef_by_dxpdefRES = new MatrixFixed(STATE_SIZE, POSITION_STATE_SIZE);
            dxvnorm_by_dxvRES = new MatrixFixed(STATE_SIZE, STATE_SIZE);
            dzeroedxv_by_dxvRES = new MatrixFixed(STATE_SIZE, STATE_SIZE);
            dxpredef_by_dxpRES = new MatrixFixed(POSITION_STATE_SIZE, POSITION_STATE_SIZE);
            dxpredef_by_dxpdefRES = new MatrixFixed(POSITION_STATE_SIZE, POSITION_STATE_SIZE);
        }

 /// <summary>
 /// Calculate trace of a matrix
 /// </summary>
 /// <param name="m"></param>
 /// <returns></returns>
 public static float Trace(MatrixFixed M)
 {
     float sum = 0;
     int N = (M.Rows < M.Columns ? M.Rows : M.Columns);
     for (int i=0; i<N; ++i)
         sum += M[i, i];
     return sum;
 }

 private void init()
 {
     m_centre = new Vector(2);
     m_C = new MatrixFixed(3, 3);
     m_Cinv = new MatrixFixed(3, 3);
     m_last_camera = new Vector(3);
     m_last_image_centred = new Vector(2);
 }

        //friend class FeatureInitInfo;
        //friend class Scene_Single;

        /// <summary>
        /// Constructor. This is protected since it is only called
        /// from FeatureInitInfo::add_particle()
        /// </summary>
        /// <param name="l">The value(s) for the free parameters \lambda represented by this particle.</param>
        /// <param name="p">The initial probability for this particle</param>
        /// <param name="MEASUREMENT_SIZE">The number of parameters representing a measurement of a feature</param>
        public Particle(Vector l, float p, uint MEASUREMENT_SIZE)
        {
            lambda = new Vector(l); 
            probability = p;

            m_z = new Vector(MEASUREMENT_SIZE);
            m_h = new Vector(MEASUREMENT_SIZE);
            m_SInv = new MatrixFixed(MEASUREMENT_SIZE, MEASUREMENT_SIZE);
        }

        // Default NULL version
        public virtual bool make_internal_measurement(Internal_Measurement_Model m,
                                              Vector v,
                                              Vector v2,
                                              MatrixFixed mt)
        {
            Debug.WriteLine("WARNING: make_internal_measurement not implemented.");

            return false;
        }

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

        //friend class Scene_Single;
        //friend class Kalman;

        public Internal_Measurement(Internal_Measurement_Model i_m_m)
        {
            internal_measurement_model = i_m_m;
            hv = new Vector(internal_measurement_model.MEASUREMENT_SIZE);
            zv = new Vector(internal_measurement_model.MEASUREMENT_SIZE);
            nuv = new Vector(internal_measurement_model.MEASUREMENT_SIZE);

            dhv_by_dxv = new MatrixFixed(internal_measurement_model.MEASUREMENT_SIZE, internal_measurement_model.get_motion_model().STATE_SIZE);
            Rv = new MatrixFixed(internal_measurement_model.MEASUREMENT_SIZE, internal_measurement_model.MEASUREMENT_SIZE);
            Sv = new MatrixFixed(internal_measurement_model.MEASUREMENT_SIZE, internal_measurement_model.MEASUREMENT_SIZE);

            // if (DEBUGDUMP) cout << "Adding Internal Measurement type " 
		    // << internal_measurement_model.internal_type << endl;
        }

        public void predict_internal_measurement(Vector xv, MatrixFixed Pxx)
        {
            internal_measurement_model.func_hv_and_dhv_by_dxv(xv);

            hv.Update(internal_measurement_model.get_hvRES());
            dhv_by_dxv.Update(internal_measurement_model.get_dhv_by_dxvRES());

            internal_measurement_model.func_Rv(hv);
            Rv.Update(internal_measurement_model.get_RvRES());

            internal_measurement_model.func_Sv(Pxx, dhv_by_dxv, Rv);
            Sv.Update(internal_measurement_model.get_SvRES());

            //if (DEBUGDUMP) cout << "Internal measurement prediction: hv " << endl 
		    //<< hv << endl;
        }

        public SearchDatum(MatrixFixed _PuInv, Vector _search_centre)
        {
            PuInv = _PuInv;
            search_centre = _search_centre;
            result_flag = false;
            result_u = 0;
            result_v = 0;

            halfwidth = (uint)(SceneLib.NO_SIGMA /
                        Math.Sqrt(PuInv[0, 0] - PuInv[0, 1] * PuInv[0, 1] / PuInv[1, 1]));
            halfheight = (uint)(SceneLib.NO_SIGMA /
                         Math.Sqrt(PuInv[1, 1] - PuInv[0, 1] * PuInv[0, 1] / PuInv[0, 0]));

            if (halfwidth > 10) halfwidth = 10;
            if (halfheight > 10) halfheight = 10;
        }

        /// <summary>
        /// Cholesky decomposition.
        /// Make cholesky decomposition of M optionally computing
        /// the reciprocal condition number.  If mode is estimate_condition, the
        /// condition number and an approximate nullspace are estimated, at a cost
        /// of a factor of (1 + 18/n).  Here's a table of 1 + 18/n:
        ///<pre>
        /// n:              3      5     10     50    100    500   1000
        /// slowdown:     7.0f    4.6    2.8    1.4   1.18   1.04   1.02
        /// </summary>
        /// <param name="M"></param>
        /// <param name="mode"></param>
        public unsafe void init(MatrixFixed M, Operation mode)
        {
            A_ = new MatrixFixed(M);

            int n = M.Columns;
            //assert(n == (int)(M.Rows()));
            num_dims_rank_def_ = -1;
            int num_dims_rank_def_temp = num_dims_rank_def_;

            // BJT: This warning is pointless - it often doesn't detect non symmetry and
            // if you know what you're doing you don't want to be slowed down
            // by a cerr
            /*
               if (Math.Abs(M[0,n-1] - M[n-1,0]) > 1e-8) 
               {
                   Debug.WriteLine("cholesky: WARNING: unsymmetric: " + M);
               }
            */

            if (mode != Operation.estimate_condition) 
            {
                // Quick factorization
                fixed (float* data = A_.Datablock())
                {
                    Netlib.dpofa_(data, &n, &n, &num_dims_rank_def_temp);                    
                }
                //if ((mode == Operation.verbose) && (num_dims_rank_def_temp != 0))
                //    Debug.WriteLine("cholesky:: " + Convert.ToString(num_dims_rank_def_temp) + " dimensions of non-posdeffness");
            } 
            else 
            {
                Vector nullvector = new Vector(n);
                float rcond_temp = rcond_;
                fixed (float* data = A_.Datablock())
                {
                    fixed (float* data2 = nullvector.Datablock())
                    {
                        Netlib.dpoco_(data, &n, &n, &rcond_temp, data2, &num_dims_rank_def_temp);
                    }
                }
                rcond_ = rcond_temp;
            }
            num_dims_rank_def_ = num_dims_rank_def_temp;
        }

        public unsafe QR(MatrixFixed M)
        {
            qrdc_out_ = new MatrixFixed(M.Columns, M.Rows);
            qraux_ = new Vector(M.Columns);
            jpvt_ = new int[M.Rows];
            Q_ = null;
            R_ = null;

            // Fill transposed O/P matrix
            int c = M.Columns;
            int r = M.Rows;
            for (int i = 0; i < r; ++i)
                for (int j = 0; j < c; ++j)
                    qrdc_out_[j,i] = M[i,j];

            int do_pivot = 0; // Enable[!=0]/disable[==0] pivoting.
            for (int i = 0; i < jpvt_.Length; i++) jpvt_[i] = 0;

            Vector work = new Vector(M.Rows);

            fixed (float* data = qrdc_out_.Datablock())
            {
                fixed (float* data2 = qraux_.Datablock())
                {
                    fixed (int* data3 = jpvt_)
                    {
                        fixed (float* data4 = work.Datablock())
                        {
                            Netlib.dqrdc_(data,       // On output, UT is R, below diag is mangled Q
                                          &r, &r, &c,
                                          data2,      // Further information required to demangle Q
                                          data3,
                                          data4,
                                          &do_pivot);
                        }
                    }
                }
            }
        }

 // Set the Jacobian \partfracv{h}{y}
 // between the feature measurement state and the feature state. 
 public void set_dh_by_dy(MatrixFixed new_dh_by_dy) { dh_by_dy.Update(new_dh_by_dy); }

 // Set the covariance, \mat{P}_{xy},  between the feature state and
 // the robot state. 
 public void set_Pxy(MatrixFixed new_Pxy) { Pxy.Update(new_Pxy); }

 /// <summary>
 /// Make a measurement of a feature.
 /// </summary>
 /// <param name="id">The identifier for this feature</param>
 /// <param name="z">The measurement of the state, to be filled in by this function</param>
 /// <param name="h">The expected measurement</param>
 /// <param name="S">The measurement covariance.</param>
 /// <returns></returns>
 public virtual bool measure_feature(byte[] id, int patchwidth, ref Vector z, Vector vz, Vector h, MatrixFixed S, Random rnd) { return (false); }

        /// <summary>
        /// Convert a partially-initialised feature to a fully-initialised feature,
        /// given information about the free parameters \vct{\lambda}.
        /// The new state \vct{y}_{fi} is given by calling
        /// Partially_Initialised_Feature_Measurement_Model::func_yfi_and_dyfi_by_dypi_and_dyfi_by_dlambda().
        /// where the various Jacobians are returned by calls to
        /// Partially_Initialised_Feature_Measurement_Model, and the covariance matrices
        /// \mat{P}_{kl} are already known and stored in the class, except for
        /// \mat{P}_{\vct{\lambda}}, which is passed to the function.
        /// </summary>
        /// <param name="lambda">The mean value for \vct{\lambda}</param>
        /// <param name="Plambda">The covariance for \vct{\lambda}</param>
        /// <param name="scene">The SLAM map</param>
        public void convert_from_partially_to_fully_initialised(
                Vector lambda, MatrixFixed Plambda, Scene_Single scene)
        {
            
            // We'll do all the work here in feature.cc though probably this only
            // works with scene_single...

            // We calculate new state yfi(ypi, lambda)
            // New feature covariance 
            // Pyfiyfi = dyfi_by_dypi Pypiypi dyfi_by_dypiT + 
            //           dyfi_by_dlambda Plambda dyfi_by_dlambdaT
            // And we change cross covariances as follows:
            // Pxyfi = Pxypi dyfi_by_dypiT
            // Pyjyfi = Pyjypi dyfi_by_dypiT   for j < i (since we only store top-right
            // Pyfiyj = dyfi_by_dypi Pypiyj    for j > i  part of covariance matrix)

            partially_initialised_feature_measurement_model.func_yfi_and_dyfi_by_dypi_and_dyfi_by_dlambda(y, lambda);

            MatrixFixed dyfi_by_dypiT = partially_initialised_feature_measurement_model.get_dyfi_by_dypiRES().Transpose();
            MatrixFixed dyfi_by_dlambdaT = partially_initialised_feature_measurement_model.get_dyfi_by_dlambdaRES().Transpose();

            // Replace y            
            y = new Vector(partially_initialised_feature_measurement_model.get_yfiRES());

            // Replace Pxy
            Pxy = Pxy * dyfi_by_dypiT;

            // Replace Pyy
            MatrixFixed Pypiypi_1 = partially_initialised_feature_measurement_model.get_dyfi_by_dypiRES() *
                        Pyy * dyfi_by_dypiT;
            MatrixFixed Pypiypi_2 = partially_initialised_feature_measurement_model.get_dyfi_by_dlambdaRES() *
                        Plambda * dyfi_by_dlambdaT;
            Pyy = Pypiypi_1 + Pypiypi_2;

            // Pyjyi elements for j < i (covariance with features before i in list)
            uint i = position_in_list;

            MatrixFixed m_it;
            int j;
            for (j = 0; j < position_in_list; j++)
            {
                m_it = (MatrixFixed)matrix_block_list[j];
                matrix_block_list[j] = m_it * dyfi_by_dypiT;
            }


            Feature it;
            int idx = scene.feature_list.IndexOf(this);
            for (j = idx + 1; j < scene.feature_list.Count; j++)
            {
                it = (Feature)(scene.feature_list[j]);
                it.matrix_block_list[(int)i] = partially_initialised_feature_measurement_model.get_dyfi_by_dypiRES() * (MatrixFixed)it.matrix_block_list[(int)i];
                it.increment_position_in_total_state_vector(-(int)feature_measurement_model.FEATURE_STATE_SIZE);
            }


            // Change the total state size in scene, here with a negative increment
            uint size1 = partially_initialised_feature_measurement_model.more_initialised_feature_measurement_model.FEATURE_STATE_SIZE;
            uint size2 = partially_initialised_feature_measurement_model.FEATURE_STATE_SIZE;
            scene.increment_total_state_size((int)size1 - (int)size2);

            // Change fmm for this model to fully-initialised one
            feature_measurement_model =
                partially_initialised_feature_measurement_model.more_initialised_feature_measurement_model;

            partially_initialised_feature_measurement_model = null;
            fully_initialised_feature_measurement_model =
                (Fully_Initialised_Feature_Measurement_Model)feature_measurement_model;


            //assert(fully_initialised_feature_measurement_model != NULL);

            // Need to reallocate any other matrices
            // Assume that measurement size doesn't change 
            dh_by_dy.Resize(feature_measurement_model.MEASUREMENT_SIZE, feature_measurement_model.FEATURE_STATE_SIZE);
                         
             
        }

 // Set the feature state covariance, \mat{P}_{yy}. 
 public void set_Pyy(MatrixFixed new_Pyy) { Pyy.Update(new_Pyy); }

        /// <summary>
        /// Function which unites common stuff for constructors below
        /// Constructor stuff which is common to more than one constructor
        /// </summary>
        protected void feature_constructor_bookeeping()
        {
            selected_flag = false;        // Feature is unselected when first detected
            scheduled_for_termination_flag = false;
            attempted_measurements_of_feature = 0;
            successful_measurements_of_feature = 0;

            // Allocate matrices for storing predicted and actual measurements
            h = new Vector(feature_measurement_model.MEASUREMENT_SIZE);
            z = new Vector(feature_measurement_model.MEASUREMENT_SIZE);
            prev_z = new Vector(feature_measurement_model.MEASUREMENT_SIZE);
            nu = new Vector(feature_measurement_model.MEASUREMENT_SIZE);

            vz = new Vector(2);
            accn_z = new Vector(2);

            dh_by_dxv = new MatrixFixed(feature_measurement_model.MEASUREMENT_SIZE, feature_measurement_model.get_motion_model().STATE_SIZE);
            dh_by_dy = new MatrixFixed(feature_measurement_model.MEASUREMENT_SIZE, feature_measurement_model.FEATURE_STATE_SIZE);
            R = new MatrixFixed(feature_measurement_model.MEASUREMENT_SIZE, feature_measurement_model.MEASUREMENT_SIZE);
            S = new MatrixFixed(feature_measurement_model.MEASUREMENT_SIZE, feature_measurement_model.MEASUREMENT_SIZE);
        }

 /// <summary>
 /// Make a measurement of a feature.
 /// </summary>
 /// <param name="id">The identifier for this feature</param>
 /// <param name="z">The measurement of the state, to be filled in by this function</param>
 /// <param name="h">The expected measurement</param>
 /// <param name="S">The measurement covariance.</param>
 /// <returns></returns>
 public virtual bool measure_feature(classimage_mono id, ref Vector z, Vector vz, Vector h, MatrixFixed S, Random rnd) { return (false); }

 public override float selection_score(MatrixFixed m)
 {
     // Always measureable for now
     return 100000.0f;
 }