[人工智能]无人驾驶学习（三）：卡尔曼滤波器 kalmanfilter

0、前言

????????下面先列出一些其他博主的卡尔曼滤波器博客链接，以方便自己学习，过段时间希望自己可以总结。

1、写卡尔曼滤波器

无人驾驶技术入门（十三）| 手把手教你写卡尔曼滤波器https://zhuanlan.zhihu.com/p/45238681

?

??????? 文件名：kalmanfilter.h

#ifndef KALMAN_FILTER_H
#define KALMAN_FILTER_H
#include "Eigen/Dense"

using namespace std;

class KalmanFilter{//定义一个卡尔曼滤波器类
public:
      //constructor:是一种用于创建和初始化“class创建的对象”的特殊方法
      KalmanFilter(){
           is_initalized_= false;
      };

      //destructor析构函数，与构造函数相反；
      //当对象结束其生命周期，如对象所在的函数已调用完毕时，系统自动执行析构函数
      //~KalmanFilter();

      void Initalization(Eigen::VectorXd x_in){
           x_= x_in;
           is_initalized_= true;
           //cout<<"intialized"<<endl;
      }

      bool IsInitalized(){
           return is_initalized_;
      }

     Eigen::VectorXd GetX(){
           return x_;
     }

     //状态转移矩阵F_（设置函数）：state transistion matrix
     //可以通过先给F_in赋值，然后调用SetF()函数对状态转移矩阵F_进行赋值
     void SetF(Eigen::MatrixXd F_in){
        F_ = F_in;
      }

     //状态协方差矩阵(设置函数)：state covariance matrix
     void SetP(Eigen::MatrixXd P_in){
        P_ = P_in;
      }

     //过程噪声矩阵(设置函数)：process covariance matrix
     void SetQ(Eigen::MatrixXd Q_in){

        Q_ = Q_in;
      }

     //x'= Fx + u。下面将u设为0，即忽略了外部的影响(将模型简化了)
     //P' = F*P*Ft + Q
     void Prediction(){
        //cout<<"prediction"<<endl;
        x_ = F_ * x_;
        //cout<<"预测值 "<<x_<<endl;
        //cout<<"预测值F "<<F_<<endl;
        Eigen::MatrixXd Ft = F_.transpose();
        P_ = F_ * P_ * Ft + Q_;
     }

     //测量矩阵:Measurement Matrix
     void SetH(Eigen::MatrixXd H_in){
        H_ = H_in;
      }

     //测量噪声矩阵:measurement covariance matrix
     void SetR(Eigen::MatrixXd R_in){
        R_ = R_in;
      }

     //预测--更新
     // y=z-Hx' 实际观测值z与预测值x'之间的差值y
     // S=H*P*Ht+R S是临时变量
     // K=P'*Ht*S.inverse() K:卡尔曼增益,是y值的权重
     void Update(const Eigen::VectorXd &z){
        // 预测
        Eigen::VectorXd y = z - H_ * x_;
        Eigen::MatrixXd S = H_ * P_ * H_.transpose() + R_;
        Eigen::MatrixXd K = P_ * H_.transpose()*S.inverse(); //卡尔曼增益

        //更新
        x_ = x_+(K*y);//更新当前状态向量
        //cout<<"更新或得值 "<<x_<<endl;
        int size = x_.size();//用于构造单位矩阵
        Eigen::MatrixXd I = Eigen::MatrixXd::Identity(size, size);
        P_ = (I - K*H_)*P_;//更新系统的不确定度
     }       

//定义参数
private:

      bool is_initalized_;//是否初始化
      Eigen::VectorXd x_; //state vector状态向量
      Eigen::MatrixXd F_; //状态转移矩阵
      Eigen::MatrixXd P_; //状态协方差
      Eigen::MatrixXd Q_; //噪声
      Eigen::MatrixXd H_; //测量矩阵
      Eigen::MatrixXd R_; //噪声

};


#endif