simplekalmanfilter.hpp

gps源代码

         */
        virtual int Compute(const double& phiValue, const double& controlGain, const double& controlInput, const double& processNoiseVariance, const double& measurement, const double& measurementsGain, const double& measurementsNoiseVariance) throw(InvalidSolver);


        /** Compute the a posteriori estimate of the system state, as well as the
         * a posteriori estimate error variance. Version for one-dimensional systems
         * without control input on the system.
         *
         * @param phiValue          State transition gain.
         * @param processNoiseVariance    Process noise variance.
         * @param measurement       Measurement value.
         * @param measurementsGain  Measurements gain.
         * @param measurementsNoiseVariance   Measurements noise variance.
         *
         * @return
         *  0 if OK
         *  -1 if problems arose
         */
        virtual int Compute(const double& phiValue, const double& processNoiseVariance, const double& measurement, const double& measurementsGain, const double& measurementsNoiseVariance) throw(InvalidSolver);


        /// Destructor.
        virtual ~SimpleKalmanFilter() {};

        /// A posteriori state estimation. This is usually your target.
        Vector<double> xhat;

        /// A posteriori error covariance.
        Matrix<double> P;

        /// A priori state estimation.
        Vector<double> xhatminus;

        /// A priori error covariance.
        Matrix<double> Pminus;


    private:


        /** Predicts (or "time updates") the a priori estimate of the system state,
         * as well as the a priori estimate error covariance matrix.
         *
         * @param phiMatrix         State transition matrix.
         * @param previousState     Previous system state vector. It is the last computed xhat.
         * @param controlMatrix     Control matrix.
         * @param controlInput      Control input vector.
         * @param processNoiseCovariance    Process noise covariance matrix.
         *
         * @return
         *  0 if OK
         *  -1 if problems arose
         */
        virtual int Predict(const Matrix<double>& phiMatrix, const Vector<double>& previousState, const Matrix<double>& controlMatrix, const Vector<double>& controlInput, const Matrix<double>& processNoiseCovariance) throw(InvalidSolver);


        /** Predicts (or "time updates") the a priori estimate of the system state,
         * as well as the a priori estimate error variance. Version for
         * one-dimensional systems.
         *
         * @param phiValue          State transition gain.
         * @param previousState     Previous system state. It is the last computed xhat.
         * @param controlGain       Control gain.
         * @param controlInput      Control input value.
         * @param processNoiseVariance    Process noise variance.
         *
         * @return
         *  0 if OK
         *  -1 if problems arose
         */
        virtual int Predict(const double& phiValue, const double& previousState, const double& controlGain, const double& controlInput, const double& processNoiseVariance) throw(InvalidSolver)
        {

            // Create dummy matrices and vectors and call the full Predict() method
            Matrix<double> dummyPhiMatrix(1,1,phiValue);
            Vector<double> dummyPreviousState(1,previousState);
            Matrix<double> dummyControMatrix(1,1,controlGain);
            Vector<double> dummyControlInput(1,controlInput);
            Matrix<double> dummyProcessNoiseCovariance(1,1,processNoiseVariance);

            return ( (*this).Predict(dummyPhiMatrix, dummyPreviousState, dummyControMatrix, dummyControlInput, dummyProcessNoiseCovariance) );

        };


        /** Predicts (or "time updates") the a priori estimate of the system state,
         * as well as the a priori estimate error covariance matrix. This version
         * assumes that no control inputs act on the system.
         *
         * @param phiMatrix         State transition matrix.
         * @param previousState     Previous system state vector. It is the last computed xhat.
         * @param processNoiseCovariance    Process noise covariance matrix.
         *
         * @return
         *  0 if OK
         *  -1 if problems arose
         */
        virtual int Predict(const Matrix<double>& phiMatrix, const Vector<double>& previousState, const Matrix<double>& processNoiseCovariance) throw(InvalidSolver)
        {

            // Create dummy matrices and vectors and call the full Predict() method
            int stateRow(previousState.size());

            Matrix<double> dummyControMatrix(stateRow,1,0.0);
            Vector<double> dummyControlInput(1,0.0);

            return ( (*this).Predict(phiMatrix, previousState, dummyControMatrix, dummyControlInput, processNoiseCovariance) );

        };


        /** Predicts (or "time updates") the a priori estimate of the system state,
         * as well as the a priori estimate error variance. Version for
         * one-dimensional systems and no control input acting on the system.
         *
         * @param phiValue          State transition gain.
         * @param previousState     Previous system state. It is the last computed xhat.
         * @param processNoiseVariance    Process noise variance.
         *
         * @return
         *  0 if OK
         *  -1 if problems arose
         */
        virtual int Predict(const double& phiValue, const double& previousState, const double& processNoiseVariance) throw(InvalidSolver)
        {

            // Create dummy matrices and vectors and call the full Predict() method
            Matrix<double> dummyPhiMatrix(1,1,phiValue);
            Vector<double> dummyPreviousState(1,previousState);
            Matrix<double> dummyControMatrix(1,1,0.0);
            Vector<double> dummyControlInput(1,0.0);
            Matrix<double> dummyProcessNoiseCovariance(1,1,processNoiseVariance);

            return ( (*this).Predict(dummyPhiMatrix, dummyPreviousState, dummyControMatrix, dummyControlInput, dummyProcessNoiseCovariance) );

        };


        /** Corrects (or "measurement updates") the a posteriori estimate of the
         * system state vector, as well as the a posteriori estimate error covariance
         * matrix, using as input the predicted a priori state vector and error
         * covariance matrix, plus measurements and associated matrices.
         *
         * @param measurements      Measurements vector.
         * @param measurementsMatrix    Measurements matrix. Called geometry matrix in GNSS.
         * @param measurementsNoiseCovariance   Measurements noise covariance matrix.
         *
         * @return
         *  0 if OK
         *  -1 if problems arose
         */
        virtual int Correct(const Vector<double>& measurements, const Matrix<double>& measurementsMatrix, const Matrix<double>& measurementsNoiseCovariance) throw(InvalidSolver);


        /** Corrects (or "measurement updates") the a posteriori estimate of the
         * system state value, as well as the a posteriori estimate error variance,
         * using as input the predicted a priori state and error variance values,
         * plus measurement and associated gain and variance. Version for
         * one-dimensional systems.
         *
         * @param measurement       Measurement value.
         * @param measurementsGain  Measurements gain.
         * @param measurementsNoiseVariance   Measurements noise variance.
         *
         * @return
         *  0 if OK
         *  -1 if problems arose
         */
        virtual int Correct(const double& measurement, const double& measurementsGain, const double& measurementsNoiseVariance) throw(InvalidSolver)
        {

            // Create dummy matrices and vectors and call the full Correct() method
            Vector<double> dummyMeasurements(1,measurement);
            Matrix<double> dummyMeasurementsMatrix(1,1,measurementsGain);
            Matrix<double> dummyMeasurementsNoise(1,1,measurementsNoiseVariance);

            return ( (*this).Correct(dummyMeasurements, dummyMeasurementsMatrix, dummyMeasurementsNoise) );

        };


    }; // class SimpleKalmanFilter


   //@}

} // namespace

#endif