基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业

基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第1张图片

1、卡尔曼滤波介绍

参考

详解卡尔曼滤波原理_清风莞尔的博客-CSDN博客_卡尔曼滤波原理

1、运动方程与观测方程

其中

代表位置和速度

Vk代表输入输入,Wk代表噪声

运动方程表示已知k-1时刻的状态和k时刻的输入来预测k时刻的状态(位姿)

当融合imu与雷达信息时,由imu得到预测即运动方程(状态方程),雷达得到观测方程

观测方程表示在k时刻已知状态xk和噪声nk观测到环境中物体yk。在此融合问题中由

2、线形高斯假设重写两个方程

运动方程

k-1时刻机器人状态服从一个高斯分布,我们根据一个变换和输入预测k时刻的状态分布;以下是k-1时刻的均值和协方差矩阵

我们要得到k时刻的状态的高斯分布要求出k时刻的均值与协方差

基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第2张图片

 用矩阵Fk来表示预测过程则

用图形表示为:

基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第3张图片

协方差矩阵变为:

基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第4张图片

添加上噪声Q后预测变为:

基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第5张图片

观测方程

 k时刻有传感器进(雷达)行了观测,所以我们可以用雷达观测的数据(也服从一个高斯分布)和imu预测的结果结合得到更精确的结果

同样的我们由观测到到数据也会产生由观测计算出来的新的状态。即知道由雷达观测,通过前端(icp,ndt,loam)等方式计算位姿

 基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第6张图片

 当传感器有一定的误差时位姿计算变为:

基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第7张图片

 3、卡尔曼滤波融合

 高斯分布融合基础

我们现在知道两个状态的高斯分布,一个是imu预测得到的状态分布,一个是雷达观测当前时刻计算出的状态的高斯分布,将两个高斯分布融合得到新的一个状态的高斯分布就是我们要的融合结果

 用图形表示如下:

基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第8张图片

粉色表示预测值绿色表示观测值,中间交集部分即为我们要的滤波后的接近真实值的值。

 基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第9张图片基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第10张图片

推导见参考博客

融合IMU与雷达 基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第11张图片

该式表示,已知初始时刻状态,和1到k时刻的输入,以及前面时刻的观测,来计算当前状态(位姿即定位任务)和观测(即建图任务)

完成定位任务 基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第12张图片

该式就融合了imu预测(x0,v1:k)和雷达观测(y0:k)从而求出新的状态(位姿进行定位)

均值和协方差矩阵计算方法参照上一部分

基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第13张图片

 再进行整理得到

基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第14张图片

 第一个式子是卡尔曼增益

第二个式子是融合之后的高斯分布的协方差矩阵

第三个式子是融合后的高斯分布的均值

第四个式子是预测状态(运动方程)的高斯分布的均值

第五个式子是预测状态的高斯分布的协方差

 状态方程构建

 基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第15张图片

 观测方程构建

基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第16张图片

 构建滤波器

基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第17张图片

 流程

基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第18张图片

 代码详解

1、更新里程计信息

void ErrorStateKalmanFilter::UpdateOdomEstimation(
    Eigen::Vector3d &linear_acc_mid, Eigen::Vector3d &angular_vel_mid) {
  //
  // TODO: this is one possible solution to previous chapter, IMU Navigation,
  // assignment
  //
  // get deltas:
  Eigen::Vector3d deltas=Eigen::Vector3d::Zero();
  size_t index_curr_=1;
  size_t index_prev_=0;
  GetAngularDelta(index_curr_,  index_prev_,   deltas,  angular_vel_mid);

  // update orientation:
  Eigen::Matrix3d R_curr_ =Eigen::Matrix3d::Identity();
  Eigen::Matrix3d R_prev_ =Eigen::Matrix3d::Identity();
  UpdateOrientation(deltas,R_curr_,R_prev_);
  // get velocity delta:
  double delta_t_=0;
  Eigen::Vector3d velocity_delta_=Eigen::Vector3d::Zero();
  GetVelocityDelta(index_curr_,index_prev_,R_curr_,R_prev_,delta_t_,velocity_delta_,linear_acc_mid);
  // save mid-value unbiased linear acc for error-state update:

  // update position:
  UpdatePosition(delta_t_, velocity_delta_);
}

这部分是上一章内容

基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第19张图片

参考

惯性传感器导航解算/多传感器融合6_dushib的博客-CSDN博客

2、求解F和B

void ErrorStateKalmanFilter::SetProcessEquation(const Eigen::Matrix3d &C_nb,
                                                const Eigen::Vector3d &f_b,
                                                const Eigen::Vector3d &w_b) {
  // TODO: set process / system equation:
  // a. set process equation for delta vel:
  F_.setZero();
  F_.block<3,  3>(kIndexErrorPos,  kIndexErrorVel)  =  Eigen::Matrix3d::Identity();
  F_.block<3,  3>(kIndexErrorVel,   kIndexErrorOri)  =  - C_nb *  Sophus::SO3d::hat(f_b).matrix();
  F_.block<3,  3>(kIndexErrorVel,   kIndexErrorAccel) =  -C_nb;
  F_.block<3,  3>(kIndexErrorOri,   kIndexErrorOri) =   - Sophus::SO3d::hat(w_b).matrix();
  F_.block<3,  3>(kIndexErrorOri,   kIndexErrorGyro) =   - Eigen::Matrix3d::Identity();
  // b. set process equation for delta ori:
  B_.setZero();
  B_.block<3,  3>(kIndexErrorVel,  kIndexNoiseGyro)  =    C_nb;
  B_.block<3,  3>(kIndexErrorOri,  kIndexNoiseGyro)  =     Eigen::Matrix3d::Identity();
  B_.block<3,  3>(kIndexErrorAccel,  kIndexNoiseBiasAccel)  =     Eigen::Matrix3d::Identity();
  B_.block<3,  3>(kIndexErrorGyro,  kIndexNoiseBiasGyro)    =     Eigen::Matrix3d::Identity();

}

在.h中定义这些变量为private防止被改动

private:
  // indices:
  static constexpr int kDimState{15};

  static constexpr int kIndexErrorPos{0};
  static constexpr int kIndexErrorVel{3};
  static constexpr int kIndexErrorOri{6};
  static constexpr int kIndexErrorAccel{9};
  static constexpr int kIndexErrorGyro{12};

  static constexpr int kDimProcessNoise{12};

  static constexpr int kIndexNoiseAccel{0};
  static constexpr int kIndexNoiseGyro{3};
  static constexpr int kIndexNoiseBiasAccel{6};
  static constexpr int kIndexNoiseBiasGyro{9};

  // dimensions:
  static constexpr int kDimMeasurementPose{6};
  static const int kDimMeasurementPoseNoise{6};

F和B初始化定义如下:

F:15*15

B:15*12

  using MatrixF=Eigen::Matrix;
  using MatrixB=Eigen::Matrix;

using用法:

c++中using的用法_YoungYangD的博客-CSDN博客_c++ using

eigen block用法:

Eigen子矩阵操作(block)_不二青衣的博客-CSDN博客_eigen.block

此函数求解F和B为写出运动方程作准备

基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第20张图片

注释:

此处每一个0都代表一个3*3的0矩阵,所以先将F声明成一个15*15的零矩阵

3、 kalman预测

void ErrorStateKalmanFilter::UpdateErrorEstimation(
    const double &T, const Eigen::Vector3d &linear_acc_mid,
    const Eigen::Vector3d &angular_vel_mid) {
  static MatrixF F_1st;
  static MatrixF F;
  // TODO: update process equation:
  UpdateProcessEquation(linear_acc_mid,angular_vel_mid);
  // TODO: get discretized process equations:
  F_1st=F_*T;
  F=MatrixF::Identity()+F_1st;

  MatrixB B=MatrixB::Zero();
  B.block<3,  3>(kIndexErrorVel,  kIndexNoiseGyro)  =      B_.block<3,  3>(kIndexErrorVel,  kIndexNoiseGyro) * T;
  B.block<3,  3>(kIndexErrorOri,  kIndexNoiseGyro)  =      B_.block<3,  3>(kIndexErrorOri,  kIndexNoiseGyro) *T;
  B.block<3,  3>(kIndexErrorAccel,  kIndexNoiseBiasAccel)  =    B_.block<3,  3>(kIndexErrorAccel,  kIndexNoiseBiasAccel)* sqrt(T);
  B.block<3,  3>(kIndexErrorGyro,  kIndexNoiseBiasGyro)  =      B_.block<3,  3>(kIndexErrorGyro,  kIndexNoiseBiasGyro)* sqrt(T);

  // TODO: perform Kalman prediction
  X_=F*X_;
  P_=F*P_*F.transpose()+B*Q_*B.transpose();
}

离散化

static MatrixF F_1st;
  static MatrixF F;
  // TODO: update process equation:
  UpdateProcessEquation(linear_acc_mid,angular_vel_mid);
  // TODO: get discretized process equations:
  F_1st=F_*T;
  F=MatrixF::Identity()+F_1st;

  MatrixB B=MatrixB::Zero();
  B.block<3,  3>(kIndexErrorVel,  kIndexNoiseGyro)  =      B_.block<3,  3>(kIndexErrorVel,  kIndexNoiseGyro) * T;
  B.block<3,  3>(kIndexErrorOri,  kIndexNoiseGyro)  =      B_.block<3,  3>(kIndexErrorOri,  kIndexNoiseGyro) *T;
  B.block<3,  3>(kIndexErrorAccel,  kIndexNoiseBiasAccel)  =    B_.block<3,  3>(kIndexErrorAccel,  kIndexNoiseBiasAccel)* sqrt(T);
  B.block<3,  3>(kIndexErrorGyro,  kIndexNoiseBiasGyro)  =      B_.block<3,  3>(kIndexErrorGyro,  kIndexNoiseBiasGyro)* sqrt(T);
// TODO: perform Kalman prediction
  X_=F*X_;
  P_=F*P_*F.transpose()+B*Q_*B.transpose();
}

基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第21张图片

运动方程

X_=F*X_;

协方差矩阵

P_=F*P_*F.transpose()+B*Q_*B.transpose();

 4、观测更新

void ErrorStateKalmanFilter::CorrectErrorEstimationPose(                    //  计算  Y ,G  ,K
    const Eigen::Matrix4d &T_nb, Eigen::VectorXd &Y, Eigen::MatrixXd &G,
    Eigen::MatrixXd &K) {
  //
  // TODO: set measurement:      计算观测 delta pos  、 delta  ori
  //
  Eigen::Vector3d  dp  =  pose_.block<3,  1>(0,  3)  -   T_nb.block<3,  1>(0,  3);
  Eigen::Matrix3d  dR  =  T_nb.block<3,  3>(0, 0).transpose() *  pose_.block<3, 3>(0, 0) ;
  // TODO: set measurement equation:
  Eigen::Vector3d  dtheta  =  Sophus::SO3d::vee(dR  -  Eigen::Matrix3d::Identity() );
  YPose_.block<3, 1>(0, 0)  =  dp;          //    delta  position 
  YPose_.block<3, 1>(3, 0)  =  dtheta;          //   失准角
  Y = YPose_;
  //   set measurement  G
  GPose_.setZero();
  GPose_.block<3, 3>(0, kIndexErrorPos)  =  Eigen::Matrix3d::Identity();
  GPose_.block<3 ,3>(3, kIndexErrorOri)   =  Eigen::Matrix3d::Identity();        
  G  =   GPose_;     
  //   set measurement  C
  CPose_.setZero();
  CPose_.block<3, 3>(0,kIndexNoiseAccel)   =  Eigen::Matrix3d::Identity();
  CPose_.block<3, 3>(3,kIndexNoiseGyro)    =  Eigen::Matrix3d::Identity();
  Eigen::MatrixXd  C  =   CPose_;
  // TODO: set Kalman gain:
  Eigen::MatrixXd R = RPose_;    //  观测噪声
  K =  P_  *  G.transpose() * ( G  *  P_  *  G.transpose( )  +  C * RPose_*  C.transpose() ).inverse() ;
}

void ErrorStateKalmanFilter::CorrectErrorEstimation(
    const MeasurementType &measurement_type, const Measurement &measurement) {
  //
  // TODO: understand ESKF correct workflow
  //
  Eigen::VectorXd Y;
  Eigen::MatrixXd G, K;
  switch (measurement_type) {
  case MeasurementType::POSE:
    CorrectErrorEstimationPose(measurement.T_nb, Y, G, K);
    break;
  default:
    break;
  }

  // TODO: perform Kalman correct:
  P_ = (MatrixP::Identity() - K*G) * P_; // fix this
  X_ = X_ + K * (Y - G*X_);              // fix this
}

基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第22张图片

详解

Eigen::Vector3d  dp  =  pose_.block<3,  1>(0,  3)  -   T_nb.block<3,  1>(0,  3);

Eigen::Matrix3d  dR  =  T_nb.block<3,  3>(0, 0).transpose() *  pose_.block<3, 3>(0, 0) ;

 

Eigen::Vector3d  dtheta  =  Sophus::SO3d::vee(dR  -  Eigen::Matrix3d::Identity() );

 

YPose_.block<3, 1>(0, 0)  =  dp;          //    delta  position 
YPose_.block<3, 1>(3, 0)  =  dtheta;          //   失准角

 基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第23张图片

 GPose_.setZero();
  GPose_.block<3, 3>(0, kIndexErrorPos)  =  Eigen::Matrix3d::Identity();
  GPose_.block<3 ,3>(3, kIndexErrorOri)   =  Eigen::Matrix3d::Identity();        
  G  =   GPose_;     

CPose_.setZero();
  CPose_.block<3, 3>(0,kIndexNoiseAccel)   =  Eigen::Matrix3d::Identity();
  CPose_.block<3, 3>(3,kIndexNoiseGyro)    =  Eigen::Matrix3d::Identity();
  Eigen::MatrixXd  C  =   CPose_;

 K =  P_  *  G.transpose() * ( G  *  P_  *  G.transpose( )  +  C * RPose_*  C.transpose() ).inverse() ;

 

 P_ = (MatrixP::Identity() - K*G) * P_; // fix this
  X_ = X_ + K * (Y - G*X_);              // fix this

 

5、 后验位姿

void ErrorStateKalmanFilter::EliminateError(void) {
  //      误差状态量  X_  :   15*1
  // TODO: correct state estimation using the state of ESKF
  //
  // a. position:
  // do it!
  pose_.block<3, 1>(0, 3)  -=  X_.block<3, 1>(kIndexErrorPos, 0 );   //  减去error
  // b. velocity:
  // do it!
  vel_ -=  X_.block<3,1>(kIndexErrorVel, 0 );         
  // c. orientation:
  // do it!
  Eigen::Matrix3d   dtheta_cross =  Sophus::SO3d::hat(X_.block<3,1>(kIndexErrorOri, 0));         //   失准角的反对称矩阵
  pose_.block<3, 3>(0, 0) =  pose_.block<3, 3>(0, 0) * (Eigen::Matrix3d::Identity() - dtheta_cross);     
  Eigen::Quaterniond  q_tmp(pose_.block<3, 3>(0, 0) );
  q_tmp.normalize();        //  为了保证旋转矩阵是正定的
  pose_.block<3, 3>(0, 0) = q_tmp.toRotationMatrix();  

  // d. gyro bias:
  if (IsCovStable(kIndexErrorGyro)) {
    gyro_bias_ -= X_.block<3, 1>(kIndexErrorGyro, 0);           //  判断gyro_bias_error是否可观
  }

  // e. accel bias:
  if (IsCovStable(kIndexErrorAccel)) {
    accl_bias_ -= X_.block<3, 1>(kIndexErrorAccel, 0);          //   判断accel_bias_error是否可观 
  }
}

基于卡尔曼滤波器的融合IMU与雷达/深蓝多传感器融合第七章作业_第24张图片

6、结果

参考

详解卡尔曼滤波原理_清风莞尔的博客-CSDN博客_卡尔曼滤波原理

c++中using的用法_YoungYangD的博客-CSDN博客_c++ using

Eigen子矩阵操作(block)_不二青衣的博客-CSDN博客_eigen.block

惯性传感器导航解算/多传感器融合6_dushib的博客-CSDN博客

深蓝学院多传感器融合课程第七章课件

深蓝学院-多传感器融合定位-第7章作业_Weiheng-Summer的博客-CSDN博客

你可能感兴趣的:(深蓝多传感器融合,自动驾驶,人工智能)