卡尔曼滤波实现C++（1D和3D）

卡尔曼滤波原理参考：轻松理解卡尔曼滤波
卡尔曼滤波的理解以及参数调整

C++源码：

#ifndef KALMAN_H
#define KALMAN_H


#include 
using Eigen::MatrixXd;
using Eigen::VectorXd;
using namespace std;
using std::vector;

class kalman_filter
{
public:
	kalman_filter()
    {
        delta_time=0;
        X=MatrixXd(2, 1);
        F=MatrixXd(2, 2);
		B=MatrixXd(2, 1);
        H=MatrixXd(1, 2);
        Q=MatrixXd(2, 2);
        R=MatrixXd(1, 1);
        P=MatrixXd(2, 2);
        K=MatrixXd(2, 1);
        X<<0,0;
        F<<1,(double)delta_time,0,1 ;
		B<<0.5*delta_time*delta_time,delta_time;
        H<<1,0;
        Q<< (double)0.01,0,0,(double)0.01;
        R<<(double)0.01;
        P<<1,0,0,1;
        K<< 0,0;
    }
	~kalman_filter(){}
	void kalman_init(MatrixXd X,
						MatrixXd F, 
						MatrixXd H, 
						MatrixXd Q,
						MatrixXd R,
						MatrixXd P, 
						MatrixXd K)
    {
        this->X = X;
        this->F = F;
        this->B = B;
        this->H = H;
        this->Q = Q;
        this->R = R;
        this->P = P;
        this->K = K;
    }
	MatrixXd kalman_input_exe(MatrixXd stata_X,MatrixXd z,double u, double time)
    {
        this->delta_time=time;
        F<<1,(double)delta_time,0,1 ;
        B<<0.5*delta_time*delta_time,delta_time;
        /* Predict */
        X=F*X+B*u;
        P=F*P*F.transpose() + Q;
        /*update*/
        K = P*H.transpose()*(H*P*H.transpose()+R).inverse();
        X = X+K*(z-H*X);
        P = P - K*H*P;
        return X;
    }

	MatrixXd X;  /* state */
	MatrixXd F;  /* x(n)=F*x(n-1)+B*u(n),u(n)~N(0,q) */
	MatrixXd B;  /*B为内部控制量*/
	MatrixXd H;  /* z(n)=H*x(n)+w(n),w(n)~N(0,r)   */
	MatrixXd Q;  /* process(predict) noise convariance */
	MatrixXd R;  /* measure noise convariance */
	MatrixXd P;  /* estimated error convariance */
	MatrixXd K;  /*kalman 增益*/
    double delta_time;
};



//X,Y,Z同时加滤波
class kalman_filter_3D
{
public:
	kalman_filter_3D()
    {
        delta_time=0;
        X=MatrixXd(6, 1);
        F=MatrixXd(6, 6);
        H=MatrixXd(6, 6);
        Q=MatrixXd(6, 6);
        R=MatrixXd(6, 6);
        P=MatrixXd(6, 6);
        K=MatrixXd(6, 6);
        X.setZero();
        X(3,0)=1;
        X(4,0)=1;
        X(5,0)=1;
        F<<1,0,0,delta_time,0,0,
        0,1,0,0,delta_time,0,
        0,0,1,0,0,delta_time,
        0,0,0,1,0,0,
        0,0,0,0,1,0,
        0,0,0,0,0,1;
        H.setIdentity();
        Q<< 0.01,0,0,0,0,0,
        0,0.01,0,0,0,0,
        0,0,0.01,0,0,0,
        0,0,0,0.01,0,0,
        0,0,0,0,0.01,0,
        0,0,0,0,0,0.01;
        R<< 0.1,0,0,0,0,0,
        0,0.1,0,0,0,0,
        0,0,0.1,0,0,0,
        0,0,0,0.1,0,0,
        0,0,0,0,0.1,0,
        0,0,0,0,0,0.1;
        // R.setOnes();
        Q=Q/1000.0;
         R=R/1000.0;
        P.setZero();
        K.setZero();
    }
	~kalman_filter_3D(){}
	void kalman_init(MatrixXd X,
						MatrixXd F, 
						MatrixXd H, 
						MatrixXd Q,
						MatrixXd R,
						MatrixXd P, 
						MatrixXd K)
    {
        this->X = X;
        this->F = F;
        this->B = B;
        this->H = H;
        this->Q = Q;
        this->R = R;
        this->P = P;
        this->K = K;
    }
	MatrixXd kalman_input_exe(MatrixXd stata_X,MatrixXd z, double time)
    {
        this->delta_time=time;
        F<<1,0,0,delta_time,0,0,
        0,1,0,0,delta_time,0,
        0,0,1,0,0,delta_time,
        0,0,0,1,0,0,
        0,0,0,0,1,0,
        0,0,0,0,0,1;
        /* Predict */
        X=F*X;
        P=F*P*F.transpose() + Q;
        /*update*/
        K = P*H.transpose()*(H*P*H.transpose()+R).inverse();
        X = X+K*(z-H*X);
        P = P - K*H*P;
        return X;
    }

	MatrixXd X;  /* state */
	MatrixXd F;  /* x(n)=F*x(n-1)+B*u(n),u(n)~N(0,q) */
	MatrixXd B;  /*B为内部控制量*/
	MatrixXd H;  /* z(n)=H*x(n)+w(n),w(n)~N(0,r)   */
	MatrixXd Q;  /* process(predict) noise convariance */
	MatrixXd R;  /* measure noise convariance */
	MatrixXd P;  /* estimated error convariance */
	MatrixXd K;  /*kalman 增益*/
    double delta_time;
};
#endif

使用时直接引用头文件，实例化调用就可以。