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

111111111112454545

分类：建模仿真

发布时间 2023.03.08阅读数 1852 评论数 0

1、卡尔曼滤波介绍

参考

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

1、运动方程与观测方程

其中

代表位置和速度

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

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

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

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

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

运动方程

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

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

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

用图形表示为：

协方差矩阵变为：

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

观测方程

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

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

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

3、卡尔曼滤波融合

高斯分布融合基础

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

用图形表示如下：

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

推导见参考博客

融合IMU与雷达

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

完成定位任务

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

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

再进行整理得到

第一个式子是卡尔曼增益

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

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

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

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

状态方程构建

观测方程构建

构建滤波器

流程

代码详解

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_);
}

这部分是上一章内容

参考

惯性传感器导航解算/多传感器融合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<double, kDimState, kDimState>;
  using MatrixB=Eigen::Matrix<double, kDimState, kDimProcessNoise>;

using用法：

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

eigen block用法：

<a href=”https://blog.csdn.net/zhangqian_shai/article/details/101756145“ title=”Eigen子矩阵操作(block)_不二青衣的博客-CSDN博客_eigen.block”>Eigen子矩阵操作(block)_不二青衣的博客-CSDN博客_eigen.block

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

注释：

此处每一个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();
}

运动方程

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
}

详解

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;          //   失准角

 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是否可观 
  }
}

6、结果

参考

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

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

<a href=”https://blog.csdn.net/zhangqian_shai/article/details/101756145“ title=”Eigen子矩阵操作(block)_不二青衣的博客-CSDN博客_eigen.block”>Eigen子矩阵操作(block)_不二青衣的博客-CSDN博客_eigen.block

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

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

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

建模仿真 IMU 卡尔曼滤波器雷达传感器融合

打赏 0

上一篇：A-LOAM线/面特征雅可比解析求导

下一篇：惯性器件加速度计不基于转台的内参标定/多传感器融合第五章作业

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

111111111112454545

1、卡尔曼滤波介绍

参考

1、运动方程与观测方程

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

运动方程

观测方程

3、卡尔曼滤波融合

高斯分布融合基础

融合IMU与雷达

状态方程构建

观测方程构建

构建滤波器

流程

代码详解

1、更新里程计信息

2、求解F和B

3、 kalman预测

离散化

运动方程

协方差矩阵

4、观测更新

详解

5、 后验位姿

6、结果

参考

为你推荐

微分先行PID控制器的实现

Webots+ROS学习记录（2）-Webots与rqt工具联合使用

机械手基础知识（1）之机械手的相机模型

pybullet---力交互接口函数

一种ROS机械臂末端执行器：vacuum真空吸盘的使用教程

Webots建模指南2 - 机器人建模

评论（0）

关于作者

111111111112454545

15

0

300

2

基于滤波的融合2(在IMU与雷达的基础上融合运动约束)/深蓝学院多传感器融合第八章作业

pcl学习

惯性器件加速度计不基于转台的内参标定/多传感器融合第五章作业

相关推荐

全网最细反步法控制（Backstepping）设计讲解！！（2）——非线性阻尼，考虑扰动系统的反步法控制总结

路径规划（二）、A*算法

最优控制问题数值方法-Legendre伪谱法/勒让德伪谱法(2)

系统辨识专题（三）——多旋翼飞行器模型参数辨识问题

PX4代码阅读笔记（一）——代码结构

(17) 隐马尔科夫模型 Hidden Markov Model，维特比算法 Viterbi Algorithm - PRML && CS229

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

5、后验位姿

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板