KalmanFilter.hpp

Go to the documentation of this file.

#pragma once
 
// std
#include <concepts>
 
// Eigen
#include <Eigen/Core>
#include <Eigen/Dense>
 
namespace controlpp
{
    
    template<class T, int NStates, int NMeasurements=1, int NInputs = 0>
    class KalmanFilter{
        private:
 
        Eigen::Matrix<T, NStates, NStates> _A;
        Eigen::Matrix<T, NMeasurements, NStates> _H;
        Eigen::Matrix<T, NStates, NInputs> _B;
        Eigen::Matrix<T, NStates, NStates> _P;
        Eigen::Matrix<T, NMeasurements, NMeasurements> _R;
        Eigen::Matrix<T, NStates, NStates> _Q;
 
        Eigen::Vector<T, NStates> _x;
 
        public:
 
        template<
            int AOptions_ = 0, int AMaxRows_ = NStates, int AMaxCols_ = NStates,
            int HOptions_ = 0, int HMaxRows_ = NMeasurements, int HMaxCols_ = NStates,
            int BOptions_ = 0, int BMaxRows_ = NStates, int BMaxCols_ = NInputs,
            int ROptions_ = 0, int RMaxRows_ = NMeasurements, int RMaxCols_ = NMeasurements,
            int POptions_ = 0, int PMaxRows_ = NStates, int PMaxCols_ = NStates,
            int QOptions_ = 0, int QMaxRows_ = NStates, int QMaxCols_ = NStates
        >
        KalmanFilter(
            const Eigen::Matrix<T, NStates, NStates, AOptions_, AMaxRows_, AMaxCols_>& A, 
            const Eigen::Matrix<T, NMeasurements, NStates, HOptions_, HMaxRows_, HMaxCols_>& H,
            const Eigen::Matrix<T, NStates, NInputs, BOptions_, BMaxRows_, BMaxCols_>& B = Eigen::Matrix<T, NStates, NInputs>::Zero(),
            const Eigen::Matrix<T, NMeasurements, NMeasurements, ROptions_, RMaxRows_, RMaxCols_>& R = Eigen::Matrix<T, NMeasurements, NMeasurements>::Identity(),
            const Eigen::Matrix<T, NStates, NStates, POptions_, PMaxRows_, PMaxCols_>& P_0 = Eigen::Matrix<T, NStates, NStates>::Identity()*static_cast<T>(1e+3),
            const Eigen::Matrix<T, NStates, NStates, QOptions_, QMaxRows_, QMaxCols_>& Q = Eigen::Matrix<T, NStates, NStates>::Identity()*static_cast<T>(1e-2),
            const Eigen::Vector<T, NStates>& x_0 = Eigen::Vector<T, NStates>::Zero()
            )
            : _A(A)
            , _H(H)
            , _B(B)
            , _P(P_0)
            , _R(R)
            , _Q(Q)
            , _x(x_0){}
 
        template<int AOptions_ = 0, int AMaxRows_ = NStates, int AMaxCols_ = NStates>
        KalmanFilter& setA(const Eigen::Matrix<T, NStates, NStates, AOptions_, AMaxRows_, AMaxCols_>& new_A){this->_A = new_A; return *this;}
 
        template<int AOptions_ = 0, int AMaxRows_ = NStates, int AMaxCols_ = NStates>
        KalmanFilter& setSystemMatrix(const Eigen::Matrix<T, NStates, NStates, AOptions_, AMaxRows_, AMaxCols_>& new_A){this->_A = new_A; return *this;}
 
        template<int HOptions_ = 0, int HMaxRows_ = NMeasurements, int HMaxCols_ = NStates>
        KalmanFilter& setH(const Eigen::Matrix<T, NMeasurements, NStates, HOptions_, HMaxRows_, HMaxCols_>& new_H){this->_H = new_H; return *this;}
        
        template<int HOptions_ = 0, int HMaxRows_ = NMeasurements, int HMaxCols_ = NStates>
        KalmanFilter& setObservationMatrix(const Eigen::Matrix<T, NMeasurements, NStates, HOptions_, HMaxRows_, HMaxCols_>& new_H){this->_H = new_H; return *this;}
 
        template<int BOptions_ = 0, int BMaxRows_ = NStates, int BMaxCols_ = NInputs>
        KalmanFilter& setB(const Eigen::Matrix<T, NStates, NInputs, BOptions_, BMaxRows_, BMaxCols_>& new_B){this->_B = new_B; return *this;}
        
        template<int BOptions_ = 0, int BMaxRows_ = NStates, int BMaxCols_ = NInputs>
        KalmanFilter& setControlInputMatrix(const Eigen::Matrix<T, NStates, NInputs, BOptions_, BMaxRows_, BMaxCols_>& new_B){this->_B = new_B; return *this;}
 
        template<int ROptions_ = 0, int RMaxRows_ = NMeasurements, int RMaxCols_ = NMeasurements>
        KalmanFilter& setR(const Eigen::Matrix<T, NMeasurements, NMeasurements, ROptions_, RMaxRows_, RMaxCols_>& new_R){this->_R = new_R; return *this;}
        
        template<int ROptions_ = 0, int RMaxRows_ = NMeasurements, int RMaxCols_ = NMeasurements>
        KalmanFilter& setMeasurementCovMatrix(const Eigen::Matrix<T, NMeasurements, NMeasurements, ROptions_, RMaxRows_, RMaxCols_>& new_R){this->_R = new_R; return *this;}
 
        template<int POptions_ = 0, int PMaxRows_ = NStates, int PMaxCols_ = NStates>
        KalmanFilter& setP(const Eigen::Matrix<T, NStates, NStates, POptions_, PMaxRows_, PMaxCols_>& new_P){this->_P = new_P; return *this;}
        
        template<int POptions_ = 0, int PMaxRows_ = NStates, int PMaxCols_ = NStates>
        KalmanFilter& setErrorCovMatrix(const Eigen::Matrix<T, NStates, NStates, POptions_, PMaxRows_, PMaxCols_>& new_P){this->_P = new_P; return *this;}
        
        template<int QOptions_ = 0, int QMaxRows_ = NStates, int QMaxCols_ = NStates>
        KalmanFilter& setQ(const Eigen::Matrix<T, NStates, NStates, QOptions_, QMaxRows_, QMaxCols_>& new_Q){this->_Q = new_Q; return *this;}
 
        template<int QOptions_ = 0, int QMaxRows_ = NStates, int QMaxCols_ = NStates>
        KalmanFilter& setProcessNoiseCovMatrix(const Eigen::Matrix<T, NStates, NStates, QOptions_, QMaxRows_, QMaxCols_>& new_Q){this->_Q = new_Q; return *this;}
 
        template<int POptions_ = 0, int PMaxRows_ = NStates, int PMaxCols_ = NStates>
        KalmanFilter& setx(const Eigen::Vector<T, NStates>& new_x){this->_x = new_x; return *this;}
 
        template<int POptions_ = 0, int PMaxRows_ = NStates, int PMaxCols_ = NStates>
        KalmanFilter& setStateVector(const Eigen::Vector<T, NStates>& new_x){this->_x = new_x; return *this;}
 
        Eigen::Vector<T, NStates> makePrediction(const Eigen::Vector<T, NInputs>& u) const {
            const Eigen::Vector<T, NStates> x_p = _A * _x + _B * u;
            return x_p;
        }
 
        void add(const Eigen::Vector<T, NMeasurements>& z, const Eigen::Vector<T, NInputs>& u = Eigen::Vector<T, NInputs>::Zero()){
            // 1. Prediction
            const Eigen::Vector<T, NStates> x_p = makePrediction(u);
            const Eigen::Matrix<T, NStates, NStates> P_p = _A * _P * _A.transpose() + _Q;
 
            // 2. Update
            // Kalman factor: K = P_p * _H.transpose() * (H * P_p * H.transpose() + R).inverse();
            // U = P_p * _H.transpose()
            // S = H * P_p * H.transpose() + R
            // K = U * S.inverse();
            // K = (S.transpose().inverse() * U.transpose()).transpose()
            //
            // > note S is already symetric, so no need to transpose
            //
            // K = (S.inverse() * U.transpose()).transpose()
            const auto U = (P_p * _H.transpose()).eval();
            const auto S1 = _H * P_p * _H.transpose() + _R;
            const auto S = ((S1 + S1.transpose()) / 2).eval();
            const Eigen::Matrix<T, NStates, NMeasurements> K = (S.llt().solve(U.transpose())).transpose();
 
            _x = x_p + K * (z - _H * x_p);
 
            const auto I = Eigen::Matrix<T, NStates, NStates>::Identity();
            _P = (I - K * _H) * P_p;
        }
 
        template<std::same_as<T> U>
        requires (NMeasurements == 1 && NInputs != 1)
        void add(const U& z, const Eigen::Vector<U, NInputs>& u = Eigen::Vector<U, NInputs>::Zero()){
            const Eigen::Vector<double, 1> v_z(z);
            this->add(v_z, u);
        }
 
        template<std::same_as<T> U>
        requires (NMeasurements != 1 && NInputs == 1)
        void add(const Eigen::Vector<U, NMeasurements>& z, const U& u = static_cast<T>(0)){
            const Eigen::Vector<double, 1> v_u(u);
            this->add(z, v_u);
        }
 
        template<std::same_as<T> U>
        requires (NMeasurements == 1 && NInputs == 1)
        void add(const U& z, const U& u = static_cast<T>(0)){
            const Eigen::Vector<double, 1> v_z(z);
            const Eigen::Vector<double, 1> v_u(u);
            this->add(v_z, v_u);
        }
 
        const Eigen::Vector<T, NStates>& estimate() const {
            return this->_x;
        }
 
        const Eigen::Matrix<T, NStates, NStates>& errorCovariance() const {
            return this->_P;
        }
 
        void reset(
            const Eigen::Vector<T, NStates>& x_0 = Eigen::Vector<T, NStates>::Zero(),
            const Eigen::Matrix<T, NStates, NStates>& P_0 = Eigen::Matrix<T, NStates, NStates>::Identity()*static_cast<T>(1e+3))
        {
            this->_P = P_0;
            this->_x = x_0;   
        }
 
    };
 
} // namespace controlpp