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

1、卡尔曼滤波介绍

参考

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

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)从而求出