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

本文详细介绍了卡尔曼滤波的基本原理,包括运动方程与观测方程,并以融合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)从而求出

### 卡尔曼滤波器融合IMU和LiDAR数据 在自动驾驶车辆或机器人领域,为了提高系统的可靠性和准确性,通常采用多种传感器的数据进行融合处理。对于IMU(惯性测量单元)和LiDAR(激光雷达)这两种不同特性的传感器而言,卡尔曼滤波是一种常用的融合策略。 #### 融合原理概述 IMU提供高频度的姿态角速度以及加速度信息,但是随着时间推移会产生累积误差;而LiDAR能够给出精确的距离读数,在静态环境中可以构建高质量的地图模型,不过更新频率较低且受环境影响较大。通过将两者的优势结合起来,可以在保持较高刷新率的同时获得较为稳定的位置估计[^3]。 #### 扩展卡尔曼滤波(EKF) 由于实际应用中的非线性特性,往往会选择扩展卡尔曼滤波来进行状态预测校正: - **状态向量定义**:包括位置(x,y,z),姿态(roll,pitch,yaw)及其变化速率等参数。 - **过程方程建模**:基于IMU输出计算当前时刻的状态转移矩阵,并考虑随机扰动项的影响。 - **观测方程建立**:利用LiDAR扫描结果匹配地图特征点得到相对位姿变换作为观测量。 - **时间更新阶段**:根据前一周期结束后的最优估计值及输入控制变量推测新的先验概率分布。 - **测量更新阶段**:当接收到新一期的LiDAR数据后调整上述猜测使之更贴近真实情况。 ```python import numpy as np def ekf_predict(state, P, F, Q): """EKF 预测步""" state_pred = F @ state P_pred = F @ P @ F.T + Q return state_pred, P_pred def ekf_update(state_pred, P_pred, z, H, R): """EKF 更新步""" y = z - (H @ state_pred) S = H @ P_pred @ H.T + R K = P_pred @ H.T @ np.linalg.inv(S) state_est = state_pred + K @ y P_est = (np.eye(len(P_pred)) - K @ H) @ P_pred return state_est, P_est ``` 此代码片段展示了简化版的一维EKF实现方式,其中`state`, `P`, `F`, `Q`, `z`, `H`, 和 `R`分别代表状态向量、协方差阵、状态转移矩阵、过程噪声协方差、观测值、观测函数雅可比矩阵以及观测噪声协方差。
评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值