【纯代码复制粘贴即可使用】卡尔曼滤波的C++实现

使用Eigen实现卡尔曼滤波。如果实现扩展卡尔曼滤波只需将H矩阵换成雅可比的求解。

#ifndef _MYKALMAN_H
#define _MYKALMAN_H
#include <Eigen\Dense>


class KalmanFilter
{
public:
    KalmanFilter(int stateSize, int measSize,int uSize);
    ~KalmanFilter() = default;
    void init(Eigen::VectorXd &x, Eigen::MatrixXd& P,Eigen::MatrixXd& R, Eigen::MatrixXd& Q);
    Eigen::VectorXd predict(Eigen::MatrixXd& A);
    Eigen::VectorXd predict(Eigen::MatrixXd& A, Eigen::MatrixXd &B, Eigen::VectorXd &u);
    void update(Eigen::MatrixXd& H,Eigen::VectorXd z_meas);
private:
    int stateSize_; //state variable's dimenssion
    int measSize_; //measurement variable's dimession
    int uSize_; //control variables's dimenssion
    Eigen::VectorXd x_;
    Eigen::VectorXd z_;
    Eigen::MatrixXd A_;
    Eigen::MatrixXd B_;
    Eigen::VectorXd u_;
    Eigen::MatrixXd P_;//coveriance
    Eigen::MatrixXd H_;
    Eigen::MatrixXd R_;//measurement noise covariance
    Eigen::MatrixXd Q_;//process noise covariance
};

KalmanFilter::KalmanFilter(int stateSize = 0, int measSize = 0, int uSize=0) :stateSize_(stateSize), measSize_(measSize), uSize_(uSize)
{
    if (stateSize_ == 0 || measSize_ == 0)
    {
        std::cerr << "Error, State size and measurement size must bigger than 0\n";
    }

    x_.resize(stateSize_);
    x_.setZero();

    A_.resize(stateSize_, stateSize_);
    A_.setIdentity();

    u_.resize(uSize_);
    u_.transpose();
    u_.setZero();

    B_.resize(stateSize_, uSize_);
    B_.setZero();

    P_.resize(stateSize_, stateSize_);
    P_.setIdentity();

    H_.resize(measSize_, stateSize_);
    H_.setZero();

    z_.resize(measSize_);
    z_.setZero();

    Q_.resize(stateSize_, stateSize_);
    Q_.setZero();

    R_.resize(measSize_, measSize_);
    R_.setZero();
}

void KalmanFilter::init(Eigen::VectorXd &x, Eigen::MatrixXd& P, Eigen::MatrixXd& R, Eigen::MatrixXd& Q)
{
    x_ = x;
    P_ = P;
    R_ = R;
    Q_ = Q;
}
Eigen::VectorXd KalmanFilter::predict(Eigen::MatrixXd& A_, Eigen::MatrixXd &B_, Eigen::VectorXd &u)
{
    A = A_;
    B = B_;
    u_ = u;
    x = A*x + B*u_;
    Eigen::MatrixXd A_T = A.transpose();
    P = A*P*A_T + Q;
    return x;
}

Eigen::VectorXd KalmanFilter::predict(Eigen::MatrixXd& A)
{
    A_ = A;
    x_ = A_*x_;
    Eigen::MatrixXd A_T = A_.transpose();
    P_ = A_*P_*A_T + Q_; 
    return x_;
}

void KalmanFilter::update(Eigen::MatrixXd& H,Eigen::VectorXd z_meas)
{
    H_ = H;
    Eigen::MatrixXd temp1, temp2,Ht;
    Ht = H_.transpose();
    temp1 = H_*P_*Ht + R_;
    temp2 = temp1.inverse();
    Eigen::MatrixXd K = P_*Ht*temp2;
    z_ = H_*x_;
    x_ = x_ + K*(z_meas-z_);
    Eigen::MatrixXd I = Eigen::MatrixXd::Identity(stateSize_, stateSize_);
    P_ = (I - K*H_)*P_;
}