EKF-SLAM初探（一）多传感器融合

参考书：概率机器人、自主移动机器人导论

common sence：
1) 高斯随机变量的任何线性变换将导致另一个高斯随机变量，如， y=ax+b y = a x + b
2) 高斯随机变量的非线性变换将生成一个非高斯随机变量，如， y=x2 y = x 2
3) 典型的非线性函数关系包括平方关系、对数关系、指数关系、三角函数关系等，对非线性系统的滤波问题，常用的处理办法是利用线性化技巧将其转化为一个近似的线性滤波问题，其中应用最广泛的方法是EKF。
4) 为什么要进行线性近似？
因为要获得雅可比矩阵，使用KF进行迭代
5) 近似得不好，有什么影响吗？
得到的雅可比与真实值差别很大
6) EKF依然建立在输入噪声和测量噪声均为高斯的前提下。高斯噪声的好处是它的e指数形式使得高斯与高斯的卷积、乘法结果依然是高斯。

---

非线性函数线性化

对非线性函数 f f 和 h h 在 X^k X ^ k 处进行泰勒展开并略去二阶及以上项，得到一个近似的线性化模型，以得到的Jacobain代替KF中的状态转移矩阵和观测矩阵。

模型：

xk=f(xk−1,uk)+wkzk=h(xk)+vk x k = f ( x k − 1 , u k ) + w k z k = h ( x k ) + v k

在工作点附近 x^k−1 x ^ k − 1 、 x˜k x ~ k ，对系统进行线性近似：

f(xk−1,uk)≈f(x^k−1,uk)+∂f(xk−1,uk)∂xk−1∣∣∣x^k−1(xk−1−x^x−1)h(xk)≈h(x˜k)+∂h(xk)∂xk∣∣∣x˜k(xk−x˜k) f ( x k − 1 , u k ) ≈ f ( x ^ k − 1 , u k ) + ∂ f ( x k − 1 , u k ) ∂ x k − 1 | x ^ k − 1 ( x k − 1 − x ^ x − 1 ) h ( x k ) ≈ h ( x ~ k ) + ∂ h ( x k ) ∂ x k | x ~ k ( x k − x ~ k )

令

Fk−1=∂f(xk−1,uk)∂xk−1∣∣∣x^k−1,Hk=∂h(xk)∂xk∣∣∣x˜k F k − 1 = ∂ f ( x k − 1 , u k ) ∂ x k − 1 | x ^ k − 1 , H k = ∂ h ( x k ) ∂ x k | x ~ k

对于高斯分布，选择线性化点的最有可能的状态，就是后验估计的均值 x^k−1 x ^ k − 1 （参考概率机器人P43）.换句话说，用 f f 在 x^k−1 x ^ k − 1 （和在 uk u k ）处的值以及斜率进行线性展开，去近似 f f .
同理，对 h h 的精确线性化，使用的线性点是 x˜k x ~ k ，机器人认为此点是该时刻最可能的状态（因为 x˜k x ~ k 是加入 uk u k 后的，所以更接近真实均值）。

预测：

x˜k=f(x^k−1,uk)P˜k=Fk−1P^k−1FTk−1+Qk−1 x ~ k = f ( x ^ k − 1 , u k ) P ~ k = F k − 1 P ^ k − 1 F k − 1 T + Q k − 1

更新：

Kk=PkHTk(HkPkHTk+R)−1x^k=x˜k+Kk(zk−h(x˜k))P^k=(I−KkHk)P˜k K k = P k H k T ( H k P k H k T + R ) − 1 x ^ k = x ~ k + K k ( z k − h ( x ~ k ) ) P ^ k = ( I − K k H k ) P ~ k

影响近似好坏的因素
EKF线性近似是否具有优势主要取决于两个因素：
1) 不确定程度
较高的不确定性将会导致结果随机变量的均值和方差估计更不精确了，如下图所示：

2) 局部非线性化程度
对同一方差不同均值的高斯分布，近似的好坏与该均值处对应的<非线性函数><局部非线性程度>有关

---

含有控制输入时协方差的处理

参考：https://www.cnblogs.com/shang-slam/p/6015081.html
对带有控制输入的状态方程（在描述运动的场合，又叫运动方程），噪声由两部分构成，以里程计位置估计中的误差模型解释，一个是里程计位置估计的协方差矩阵，另一个是运动增量的协方差矩阵（参考：《自主移动机器人导论》）

---

EKF多传感器融合

1. 多传感器融合：温度测量
参考：https://home.wlu.edu/~levys/kalman_tutorial/
这个教程中的part13~part14解释的非常到位，这个例子中使用两个传感器同时测量温度，然后进行KF融合，下面是这个教程的亮点：

• 因为我们没有关于状态转移矩阵的知识（比如，自由落体、飞行器的控制输入等），所以我们只能依靠传感器的测量，所以我们只能假设 A=1 A = 1
• Pk P k 是一维的，这是因为状态变量只有一维， Gk G k 是1×2矩阵, 这是因为状态估计 x^k x ^ k 与两个传感器观测相联系
• 因为忽略了过程噪声 Q Q ，使得求解陷入麻烦，主要是导致 Pk P k 一直是0，所以我们加入过程噪声的协方差矩阵

2. 多传感器融合：lidar与radar
参考：https://blog.csdn.net/Young_Gy/article/details/78468153
1) 运动方程协方差的处理
状态变量是 x=(px,py,vx,vy) x = ( p x , p y , v x , v y ) ， Q Q 表示 x′=Fx x ′ = F x 未能刻画的其他外界干扰。例子中的运动方程使用线性模型，因此加速度变成了干扰项。 x′=Fx x ′ = F x 中未衡量的额外项目 v v 为：

v=⎡⎣⎢⎢⎢⎢⎢⎢axdt22aydt22axdtaydt⎤⎦⎥⎥⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢⎢dt220dt00dt220dt⎤⎦⎥⎥⎥⎥⎥[axay]=Ga v = [ a x d t 2 2 a y d t 2 2 a x d t a y d t ] = [ d t 2 2 0 0 d t 2 2 d t 0 0 d t ] [ a x a y ] = G a

v v 服从高斯分布 N(0,Q) N ( 0 , Q )

Q=E[vvT]=E[GaaTGT]=GE[aaT]GT=G[σ2ax00σ2ay]GT=⎡⎣⎢⎢⎢⎢⎢⎢⎢dt44σ2ax0dt32σ2ax00dt44σ2ay0dt32σ2aydt32σ2ax0dt2σ2ax00dt32σ2ay0dt2σ2ay⎤⎦⎥⎥⎥⎥⎥⎥⎥ Q = E [ v v T ] = E [ G a a T G T ] = G E [ a a T ] G T = G [ σ a x 2 0 0 σ a y 2 ] G T = [ d t 4 4 σ a x 2 0 d t 3 2 σ a x 2 0 0 d t 4 4 σ a y 2 0 d t 3 2 σ a y 2 d t 3 2 σ a x 2 0 d t 2 σ a x 2 0 0 d t 3 2 σ a y 2 0 d t 2 σ a y 2 ]

2) radar观测线性化
文中的radar使用的是极坐标系，可检测到距离、角度、速度信息，其测量值是： z=(ρ,ϕ,ρ˙) z = ( ρ , ϕ , ρ ˙ ) ，那么radar的观测方程就是非线性的，如下：

h(x)=⎡⎣⎢ρϕρ˙⎤⎦⎥=⎡⎣⎢⎢⎢⎢⎢p2x+p2y−−−−−−√arctanpypxpxvx+pyvyp2x+p2y√⎤⎦⎥⎥⎥⎥⎥ h ( x ) = [ ρ ϕ ρ ˙ ] = [ p x 2 + p y 2 arctan ⁡ p y p x p x v x + p y v y p x 2 + p y 2 ]

非线性映射线性化后的Jacob矩阵 Hj H j

Hj=∂f(x)∂x=⎡⎣⎢⎢⎢⎢⎢∂ρ∂px∂ϕ∂px∂ρ˙∂px∂ρ∂py∂ϕ∂py∂ρ˙∂py∂ρ∂vx∂ϕ∂vx∂ρ˙∂vx∂ρ∂vy∂ϕ∂vy∂ρ˙∂vy⎤⎦⎥⎥⎥⎥⎥ H j = ∂ f ( x ) ∂ x = [ ∂ ρ ∂ p x ∂ ρ ∂ p y ∂ ρ ∂ v x ∂ ρ ∂ v y ∂ ϕ ∂ p x ∂ ϕ ∂ p y ∂ ϕ ∂ v x ∂ ϕ ∂ v y ∂ ρ ˙ ∂ p x ∂ ρ ˙ ∂ p y ∂ ρ ˙ ∂ v x ∂ ρ ˙ ∂ v y ]

3) 融合策略

• lidar和radar的预测部分是完全相同的
• lidar和radar的参数更新部分是不同的，不同的原因是不同传感器收到的测量值是不同的
• 当收到lidar或radar的测量值，依次执行预测、更新步骤
• 当同时收到lidar和radar的测量值，依次执行预测、更新1、更新2步骤

---

EKF局限性

• 当强非线性时EKF违背局部线性假设，Taylor展开式中被忽略的高阶项带来大的误差时，EKF算法可能会使滤波发散；
• 由于EKF在线性化处理时需要用雅克比(Jacobian)矩阵，其繁琐的计算过程导致该方法实现相对困难

---

<完>
@leatherwang

---