KalmanFilterBase.h

#pragma once
 
#include "StateSpaceUtil.h"
 
#include <Eigen/Core>
 
namespace filters {

template <int stateDim, int inputDim> class KalmanFilterBase {
public:
    KalmanFilterBase()
        : P(Eigen::Matrix<double, stateDim, stateDim>::Identity() * 1e5),
          xHat(Eigen::Matrix<double, stateDim, 1>::Zero()) {}

    virtual void predict(const Eigen::Matrix<double, inputDim, 1>& input) = 0;

    void reset() {
        reset(Eigen::Matrix<double, stateDim, 1>::Zero());
    }

    void reset(const Eigen::Matrix<double, stateDim, 1>& state) {
        Eigen::Matrix<double, stateDim, stateDim> newP =
            Eigen::Matrix<double, stateDim, stateDim>::Identity() * 1e5;
        reset(state, newP);
    }

    void reset(const Eigen::Matrix<double, stateDim, 1>& state,
               const Eigen::Matrix<double, stateDim, 1>& stdDevs) {
        reset(state, statespace::createCovarianceMatrix(stdDevs));
    }

    void reset(const Eigen::Matrix<double, stateDim, 1>& state,
               const Eigen::Matrix<double, stateDim, stateDim>& estCovMat) {
        xHat = state;
        P = estCovMat;
    }

    Eigen::Matrix<double, stateDim, 1> getState() const {
        return xHat;
    }

    Eigen::Matrix<double, stateDim, stateDim> getEstimateCovarianceMat() const {
        return P;
    }
 
protected:
    Eigen::Matrix<double, stateDim, stateDim> P;
    Eigen::Matrix<double, stateDim, 1> xHat;
};
 
} // namespace filters