多传感器融合定位之基于滤波的融合
- 前言
- 一、贝叶斯滤波器
- 二、高斯滤波器
- 1.卡尔曼滤波(KF)
- 2.扩展卡尔曼滤波(EKF)
- 3.迭代扩展卡尔曼滤波(IEKF)
- 4.无迹卡尔曼滤波(UKF)
- 三、基于误差状态的滤波ESKF
前言
目前融合定位常用的方法主要分为两大块，一块是基于滤波的方法，另一块是基于图优化的方法。本节主要针对滤波方法展开详细介绍。
一、贝叶斯滤波器
滤波器：可以理解为在一堆历史数据(输入、观测、初始状态)下求最终融合结果即条件概率；
在一阶马尔科夫(即当前状态只跟前一时刻状态相关和其他历史时刻状态无关)假设下，利用贝叶斯公式简化滤波器条件概率，最终得到贝叶斯滤波器的后验概率即x状态值=常数 * 观测 * ∫(预测*先验即x-1状态量)dx-1
二、高斯滤波器
当变量分布采用高斯分布假设，此时贝叶斯滤波即为高斯滤波，再根据每个运动、观测方程中高斯分布传递的方式又可以划分为KF、EKF、IEKF、UKF。
1.卡尔曼滤波(KF)
如果采用线性模型传递误差，此刻高斯滤波即为卡尔曼滤波KF，具体推导流程如下：
首先，假设上一时刻后验满足正态分布，随后利用运动方程计算出当前预测值(上一时刻观测值下)的均值和方差即KF中的观测方程；
接下来，再根据当前状态和观测变量(上一时刻观测值下)的联合概率满足正态分布(公式成立的前提)推导出当前状态后验(当前观测值下)概率分布即获取到后验均值和方差表达式(跟上一时刻观测值下的当前预测值均值、方差和上一时刻观测值下的当前观测值均值、方差有关。其中上一时刻观测值下的当前观测值均值、方差=上一时刻观测值下的当前预测值变换到观测值即借助观测方程得到均值、方差)=KF中的更新方程;
上述后验为高斯滤波的通用表达式，将KF观测方程线性变换的均值和方差求出后，代入高斯滤波通用表达式，此时KF更新公式为：
2.扩展卡尔曼滤波(EKF)
如果采用非线性模型传递误差，此刻高斯滤波即为EKF，具体推导流程如下：
首先，因为运动和观测方程均为非线性，所以高斯分布处理后不再是高斯分布，为了便于运算就需要将上一时刻观测值下的当前最终预测即运动方程在上一时刻后验下一阶泰勒展开、上一时刻观测值下的当前观测值=上一时刻观测值下的当前最终预测值变换到观测值即观测方程需要在当前非线性初始预测值下一阶泰勒展开。
随后，将两个方程线性化保证输出为高斯分布，进而可以得到各自近似后的均值和方差。
最后，根据EK中当前状态后验(当前观测值下)概率分布的后验均值和方差表达式发现，二者均与运动和观测方程均值和方差有关，所以忽略均值中的小数项(例如当前非线性初始预测值=当前最终预测值),再将非线性均值和一阶泰勒展开算出的方差代入便可得到EKF更新方程。
3.迭代扩展卡尔曼滤波(IEKF)
与EKF的推导对比，IEKF其他保持不变，仅改变观测的线性化工作点，则有
按照与之前同样的方式进行推导，可得到滤波的更新过程为
具体IEKF滤波过程中，先利用状态方程算出当前预测的均值和方差，随后，考虑到观测方程在预测位置展开未必准确，可能会导致误差较大。因此，先在预测位置展开算出第一次后验以后，在第一次后验值上继续展开观测方程更新得到第二次后验，反复执行上面3个滤波更新公式，以上次的后验作为本次观测的线性化工作点，即可达到减小非线性误差的目的。
4.无迹卡尔曼滤波(UKF)
如果采样一部分sigmapoint点，传入非线性函数传递误差估计分布，此刻高斯滤波即为UKF，具体推导流程如下：
先利用上一时刻后验的均值和概率采样出若干个sigma点，之后每个sigma点代入运动方程精确求解出上一时刻观测值下的当前预测sigma个值并统计出其均值和方差。
之后，再利用上一时刻观测值下的当前预测的均值和方差采样出若干个sigma点，再将每个sigma点代入观测方程得到上一时刻观测值下的当前预测值变换到观测值并统计出其均值和方差
最后，根据EK中当前状态后验(当前观测值下)概率分布的后验均值和方差表达式发现二者均与运动和观测方程均值和方差有关，所以将上一时刻观测值下的当前预测和上一时刻观测值下的当前观测值算出的均值和方差代入便可得到UKF更新方程。
三、基于误差状态的滤波ESKF
传统KF基于导航信息融合IMU和gps/odom时效果不是很好。因此，本文利用IMU误差微分方程，基于KF提出一种精度更高的融合框架ESKF专门紧耦合IMU和gps/odom。
首先，根据上一节得到的IMU误差微分方程如下：
导航系n失准角的导数=导航系n失准角 * 导航系(n系)相对于惯性系(i系–地球系)的旋转 - imu的所有误差此处只用bias偏差(在导航坐标系n)、速度真实值与理想值之间偏差的导数(在导航坐标系n)=加速度理想值(在导航坐标系n)反对称矩阵*导航坐标系n的误差角+加速度真实值与理想值之间偏差此处只用bias偏差(在导航坐标系n)、位置真实值与理想值之间偏差的导数=速度真实值与理想值之间偏差；
随后，将位置、速度的真实值与理想值之间偏差、失准角和加速度bias、角速度bias看作状态量，化简IMU误差微分方程便可得到状态量导数与状态量之间的线性关系即状态方程；
接下来，将位置真实值与理想值之间偏差、失准角看做观测量，便可得到状态量变换到观测量的线性关系即观测方程；
最后把状态、观测方程融入到KF中并对连续方程离散化便可构建滤波器。
具体ESKF计算流程如下：
首先，将上一时刻后验状态量即误差项=0代入到状态方程中得到状态导数再乘以周期T+I，便可预测出当前状态量相对上一时刻状态量的比例系数，之后将上一时刻后验状态量=0与比例系数相乘便可得到当前状态量的预测值即误差的预测值。
之后，利用观测方程将误差预测值变换到观测量(位置真实值与理想值之间偏差、失准角)再与实际观测量(由当前imu导航方程在上一时刻后验位姿基础上解算出位置和点云匹配位置得到位置误差观测、当前imu导航方程在上一时刻后验位姿基础上解算出角度和点云匹配角度得到失准角观测)一并代入KF公式，更新出后验误差项即可。
接下来，利用后验误差和当前imu导航方程解算出的真实值得到后验理想导航值输出，因为后面再预测都会基于最优后验位姿进行开展，因此将状态误差量设置=0。后续再利用后验理想导航值+imu惯性解算=imu在下一时刻解算出的位置和角度，在当前后验状态量即误差项=0基础上得到比例系数进而得到下一时刻状态量预测值，之后将预测状态量转换观测与真实观测比对更新得到下一时刻后验状态进而得到后验位姿，不断重复步骤即可。
(fast-LIO就利用IEKF思想，不过迭代过程采用IEKF与ESKF基础思想略有不同。本节重点把最基础的ESKF进行讲解，后期有精力会专门针对fast-LIO中的IEKF再进行介绍)