无人驾驶学习（三）：卡尔曼滤波器 kalmanfilter

biter0088

已于 2022-04-09 23:04:28 修改

阅读量805

点赞数

分类专栏：无人驾驶车辆学习文章标签：自动驾驶 ubuntu c++

于 2022-02-13 23:04:55 首次发布

本文链接：https://blog.csdn.net/BIT_HXZ/article/details/122915891

版权

无人驾驶车辆学习专栏收录该内容

45 篇文章 54 订阅

订阅专栏

本文详细介绍了卡尔曼滤波器的原理，并提供了一个C++实现的卡尔曼滤波器类。通过类的成员函数，可以设置和更新滤波器的状态转移矩阵、状态协方差矩阵、过程噪声矩阵、测量矩阵和测量噪声矩阵，进而进行预测和更新操作。文章适合于对卡尔曼滤波有一定基础的读者，帮助他们深入理解和应用卡尔曼滤波算法。

摘要由CSDN通过智能技术生成

1、写卡尔曼滤波器

参考下面链接，在链接的基础上给出注释，写出卡尔曼滤波器为：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

参考链接：

无人驾驶技术入门（十三）| 手把手教你写卡尔曼滤波器 - 知乎

@meng