卡尔曼滤波预测和更新两部分，卡尔曼滤波主要实现预测过程个更新过程；因此在代码实现的时候需要完成四部分

1. 变量定义
   2）变量形状设置(由构造函数实现)
   3） 初始化，送入初始的状态量和协方差
   4） 预测过程
   使用状态转化方程完成状态量的先验估计，然后根据初始的协方差更新先验估计的协方差；
   5）更新过程
   首先需要从系统中得到当前时刻的观测量(该值是直接从系统中得到的，不需要再代码计算，因为是使用卡尔曼增益来平衡真实观察量与先验估计值的比例）；这里需要观测矩阵，和观测噪声的协方差；

#define _MYKALMAN_H
#define _MYKALMAN_H
#include<iostream>
#include <Eigen\Dense>class KalmanFilter
{
private:int stateSize; //state variable's dimenssionint measSize; //measurement variable's dimessionint uSize; //control variables's dimenssionEigen::VectorXd x;  //状态量Eigen::VectorXd z;  //观测量Eigen::MatrixXd A;  //状态转移矩阵Eigen::MatrixXd B;  //控制矩阵Eigen::VectorXd u;  //输入矩阵Eigen::MatrixXd P;  //先验估计协方差Eigen::MatrixXd H;  //观测矩阵Eigen::MatrixXd R;  //测量噪声协方差Eigen::MatrixXd Q;  //过程噪声协方差
public:KalmanFilter(int stateSize_, int measSize_, int uSize_);~KalmanFilter() {}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);
};KalmanFilter::KalmanFilter(int stateSize_ = 0, int measSize_ = 0, int uSize_ = 0) :stateSize(stateSize_), measSize(measSize_), uSize(uSize_)
// sateSize状态量个数
// 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;//根据时刻t-1的状态由状态转换举着预测时刻t的先验估计值，//因为当前是使用系统的状态转移矩阵来进行预测时刻t的预测值，并不清楚过程噪声如何，所以不适用过程噪声Eigen::MatrixXd A_T = A.transpose();P = A * P * A_T + Q;//时刻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;//  cout << "P-=" << P<< endl;return x;
}void KalmanFilter::update(Eigen::MatrixXd& H_, Eigen::VectorXd z_meas)
// H_   观测矩阵
//z_means 观测量，由系统真实观测输入
{H = H_;Eigen::MatrixXd temp1, temp2, Ht;Ht = H.transpose();temp1 = H * P * Ht + R;temp2 = temp1.inverse();//(H*P*H'+R)^(-1)Eigen::MatrixXd K = P * Ht * temp2;z = H * x;x = x + K * (z_meas - z);Eigen::MatrixXd I = Eigen::MatrixXd::Identity(stateSize, stateSize);//Identity()单位阵P = (I - K * H) * P;//  cout << "P=" << P << endl;
}