SLAM 专栏收录该内容
3 篇文章 0 订阅

# 2 状态估计

• 控制数据是机器人记录自身运动的传感器获取的数据，比如IMU中的陀螺仪可以测量角速度、加速度计可以测量运动的加速度。

• 测量数据则是机器人记录环境信息的传感器获取的数据，比如相机可以将环境转化为二维的图像像素、激光雷达捕捉环境中的信息生成点云。

x k = f ( x k − 1 , u k ) + ε k \boldsymbol{x}_{k}=f\left(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}\right)+\boldsymbol{\varepsilon}_{k}

z k = h ( x k ) + δ k \boldsymbol{z}_{k}=h\left(\boldsymbol{x}_{k}\right)+\boldsymbol{\delta}_{k}

b e l ( x k ) = p ( x k ∣ z 1 : k , u 1 : k ) bel(\boldsymbol{x}_{k})=p\left(\boldsymbol{x}_{k} | \boldsymbol{z}_{1: k}, \boldsymbol{u}_{1: k}\right)

b e l ‾ ( x k ) = p ( x k ∣ z 1 : k − 1 , u 1 : k ) \overline{bel}(\boldsymbol{x}_{k})=p\left(\boldsymbol{x}_{k} | \boldsymbol{z}_{1: k-1}, \boldsymbol{u}_{1: k}\right)

# 3 贝叶斯滤波

p ( x k ∣ z 1 : k , u 1 : k ) = p ( z k ∣ x k , z 1 : k − 1 , u 1 : k ) p ( x k ∣ z 1 : k − 1 , u 1 : k ) p ( z k ∣ z 1 : k − 1 , u 1 : k ) p\left(\boldsymbol{x}_{k} | \boldsymbol{z}_{1: k}, \boldsymbol{u}_{1: k}\right)=\frac{p\left(\boldsymbol{z}_{k} | \boldsymbol{x}_{k}, \boldsymbol{z}_{1: k-1}, \boldsymbol{u}_{1: k}\right) p\left(\boldsymbol{x}_{k} | \boldsymbol{z}_{1: k-1}, \boldsymbol{u}_{1: k}\right)}{p\left(\boldsymbol{z}_{k} | \boldsymbol{z}_{1: k-1}, \boldsymbol{u}_{1: k}\right)}

p ( x k ∣ z 1 : k , u 1 : k ) = η p ( z k ∣ x k , z 1 : k − 1 , u 1 : k ) p ( x k ∣ z 1 : k − 1 , u 1 : k ) p\left(\boldsymbol{x}_{k} | \boldsymbol{z}_{1: k}, \boldsymbol{u}_{1: k}\right)=\eta p\left(\boldsymbol{z}_{k} | \boldsymbol{x}_{k}, \boldsymbol{z}_{1: k-1}, \boldsymbol{u}_{1: k}\right) p\left(\boldsymbol{x}_{k} | \boldsymbol{z}_{1: k-1}, \boldsymbol{u}_{1: k}\right)

b e l ( x k ) = η p ( z k ∣ x k ) b e l ‾ ( x k ) bel(\boldsymbol{x}_{k})=\eta p\left(\boldsymbol{z}_{k} | \boldsymbol{x}_{k}\right) \overline{bel}(\boldsymbol{x}_{k})

b e l ‾ ( x k ) = p ( x k ∣ z 1 : k − 1 , u 1 : k ) = ∫ p ( x k ∣ x k − 1 , z 1 : k − 1 , u 1 : k ) p ( x k − 1 ∣ z 1 : k − 1 , u 1 : k ) d x k − 1 \begin{aligned} \overline{bel}\left(\boldsymbol{x}_{k}\right) &=p\left(\boldsymbol{x}_{k} | \boldsymbol{z}_{1: k-1}, \boldsymbol{u}_{1: k}\right) \\ &=\int p\left(\boldsymbol{x}_{k} | \boldsymbol{x}_{k-1}, \boldsymbol{z}_{1: k-1}, \boldsymbol{u}_{1: k}\right) p\left(\boldsymbol{x}_{k-1} | \boldsymbol{z}_{1: k-1}, \boldsymbol{u}_{1: k}\right) d\boldsymbol{x}_{k-1} \end{aligned}

b e l ‾ ( x k ) = ∫ p ( x k ∣ x k − 1 , u k ) b e l ( x k − 1 ) d x k − 1 \overline{bel}\left(\boldsymbol{x}_{k}\right) = \int p\left(\boldsymbol{x}_{k} | \boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}\right) bel(\boldsymbol{x}_{k-1}) d\boldsymbol{x}_{k-1}

Algorithm Bayes_filter( b e l ( x k − 1 ) , u k , z k bel(\boldsymbol{x}_{k-1}),\boldsymbol{u}_{k},\boldsymbol{z}_{k} ):
for all x k \boldsymbol{x}_{k} do
b e l ‾ ( x k ) = ∫ p ( x k ∣ x k − 1 , u k ) b e l ( x k − 1 ) d x k − 1 \overline{bel}\left(\boldsymbol{x}_{k}\right) = \int p\left(\boldsymbol{x}_{k} | \boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}\right) bel(\boldsymbol{x}_{k-1}) d\boldsymbol{x}_{k-1}
b e l ( x k ) = η p ( z k ∣ x k ) b e l ‾ ( x k ) bel(\boldsymbol{x}_{k})=\eta p\left(\boldsymbol{z}_{k} | \boldsymbol{x}_{k}\right) \overline{bel}(\boldsymbol{x}_{k})
end for
return b e l ( x k ) bel(\boldsymbol{x}_{k})

# 4 卡尔曼滤波

x k = f ( x k − 1 , u k ) + ε k = A k x k − 1 + B k u k + ε k \begin{aligned} \boldsymbol{x}_{k} &= f\left(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}\right)+\boldsymbol{\varepsilon}_{k} \\&=\boldsymbol{A}_{k} \boldsymbol{x}_{k-1} + \boldsymbol{B}_{k} \boldsymbol{u}_{k}+ \boldsymbol{\varepsilon}_{k} \end{aligned}

z k = h ( x k ) + δ k = C k x k + δ k \begin{aligned}\boldsymbol{z}_{k} &= h\left(\boldsymbol{x}_{k}\right)+\boldsymbol{\delta}_{k} \\&=\boldsymbol{C}_{k} \boldsymbol{x}_{k} + \boldsymbol{\delta}_{k}\end{aligned}

ε k ∼ N ( 0 , R k ) , δ k ∼ N ( 0 , Q k ) \boldsymbol{\varepsilon}_{k} \sim N\left(0, \boldsymbol{R}_{k}\right), \boldsymbol{\delta}_{k} \sim N\left(0, \boldsymbol{Q}_{k}\right)

p ( x k ∣ x k − 1 , u k ) ∼ N ( A k x k − 1 + B k u k , R k ) p(\boldsymbol{x}_{k} | \boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}) \sim N\left(\boldsymbol{A}_{k} \boldsymbol{x}_{k-1} + \boldsymbol{B}_{k} \boldsymbol{u}_{k}, \boldsymbol{R}_{k}\right)

p ( z k ∣ x k ) ∼ N ( C k x k , Q k ) p\left(\boldsymbol{z}_{k} | \boldsymbol{x}_{k}\right) \sim N\left(\boldsymbol{C}_{k} \boldsymbol{x}_{k}, \boldsymbol{Q}_{k}\right)

Algorithm Kalman_filter( μ k − 1 , Σ k − 1 , u k , z k \boldsymbol{\mu}_{k-1},\boldsymbol{\Sigma}_{k-1},\boldsymbol{u}_{k},\boldsymbol{z}_{k} ):
for all x k \boldsymbol{x}_{k} do
μ ˉ k = A k μ k − 1 + B k u k \boldsymbol{\bar{\mu}}_{k}=\boldsymbol{A}_{k} \boldsymbol{\mu}_{k-1}+\boldsymbol{B}_{k} \boldsymbol{u}_{k}
Σ ˉ k = A k Σ k − 1 A k T + R k \boldsymbol{\bar{\Sigma}}_{k}=\boldsymbol{A}_{k} \Sigma_{k-1} \boldsymbol{A}_{k}^{T}+R_{k}

K k = Σ ˉ k C k T ( C k Σ ˉ k C k T + Q k ) − 1 \boldsymbol{K}_{k}=\boldsymbol{\bar{\Sigma}}_{k} \boldsymbol{C}_{k}^{T}\left(\boldsymbol{C}_{k} \boldsymbol{\bar{\Sigma}}_{k} \boldsymbol{C}_{k}^{T}+\boldsymbol{Q}_{k}\right)^{-1}
μ k = μ ˉ k + K k ( z k − C k μ ˉ k ) \boldsymbol{\mu}_{k} = \boldsymbol{\bar{\mu}}_{k} + \boldsymbol{K}_{k}\left(\boldsymbol{z}_{k}-\boldsymbol{C}_{k} \boldsymbol{\bar{\mu}}_{k}\right)
Σ k = ( I − K k C k ) Σ ˉ k \boldsymbol{\Sigma}_{k}=\left(\boldsymbol{I}-\boldsymbol{K}_{k} \boldsymbol{C}_{k}\right) \boldsymbol{\bar{\Sigma}}_{k}
end for
return μ k \boldsymbol{\mu}_{k} , Σ k \boldsymbol{\Sigma}_{k}

# 5 举个栗子

—— 取平均。

——加权平均。

—— 把模型算出来的值，和传感器测出的值，（就像两个传感器那样），取加权平均。
OK，最后一点说明：你的模型其实只是一个步长的，也就是说，知道x(k)，我可以求x(k+1)。问题是x(k)是多少呢？答案：x(k)就是你上一步卡尔曼滤波得到的、所谓加权平均之后的那个、对x在k时刻的最佳估计值。

“SLAM是一道光，滤到你发慌”！

1.《视觉SLAM十四讲：从理论到实践 第2版》高翔等人著

2.《概率机器人》Sebastian Thrun等人著

3.《策略不给力？来一发卡尔曼滤波》知乎Fitz Hoo文章

4.《如何通俗并尽可能详细地解释卡尔曼滤波？》知乎Kent Zeng回答

5.《图说卡尔曼滤波，一份通俗易懂的教程》知乎论智文章

• 1
点赞
• 0
评论
• 1
收藏
• 一键三连
• 扫一扫，分享海报

12-09 727
04-06 6万+
06-11
03-18 24万+
09-08
04-29
03-31
11-04
05-30