This blog is translated from https://www.kalmanfilter.net/default.aspx.
It’s an excellent tutorial about Kalman Filter written by Alex Becker.
目录
卡尔曼滤波器(1) – 背景知识
卡尔曼滤波器(2) – α−β−γ滤波器(例1)
卡尔曼滤波器(3) – α−β−γ滤波器(例2)
卡尔曼滤波器(4) – α−β−γ滤波器(例3&例4&总结)
卡尔曼滤波器(5) – 一维卡尔曼滤波器(介绍)
卡尔曼滤波器(6) – 一维卡尔曼滤波器(例5&完整模型)
卡尔曼滤波器(7) – 一维卡尔曼滤波器(例6)
卡尔曼滤波器(8) – 一维卡尔曼滤波器(例7&例8)
卡尔曼滤波器(9) – 多维卡尔曼滤波器(前言&预备)
卡尔曼滤波器(10) – 多维卡尔曼滤波器(状态外推方程)
卡尔曼滤波器(11) – 多维卡尔曼滤波器(线性动态系统模型)
卡尔曼滤波器(12) – 多维卡尔曼滤波器(协方差外推方程)
卡尔曼滤波器(13) – 多维卡尔曼滤波器(测量方程)
卡尔曼滤波器(14) – 多维卡尔曼滤波器(简短总结)
卡尔曼滤波器(15) – 多维卡尔曼滤波器(状态更新方程)
卡尔曼滤波器(16) – 多维卡尔曼滤波器(协方差更新方程)
卡尔曼滤波器(17) – 多维卡尔曼滤波器(卡尔曼增益)
卡尔曼滤波器(18) – 多维卡尔曼滤波器(简化的协方差更新方程)
卡尔曼滤波器(19) – 多维卡尔曼滤波器(总结)
卡尔曼滤波器(20) – 多维卡尔曼滤波器(例9)
这是多维卡尔曼滤波器的最后一部分
有2个例子,第1个是无控制输入的六维卡尔曼滤波器,第2个是有控制输入的二维卡尔曼滤波器。
例9 - 车子位置估计
这个例子里将会使用我们目前学过的知识,讲一下多维卡尔曼滤波器的实现方法。
这里我们来估计车子在
X
Y
XY
XY 平面的位置。
车子有个板载定位传感器,可以报告系统的 X X X 和 Y Y Y 坐标。
假设系统是恒定加速度的。
状态外推方程
现在我们可以导出状态外推方程。 矩阵表示法中状态外推方程的一般形式为:
x
^
n
+
1
,
n
=
F
x
^
n
,
n
+
G
u
n
+
w
n
\pmb{\hat{x}_{n+1,n}} = \pmb{F}\pmb{\hat{x}_{n,n}} + \pmb{Gu_n} + \pmb{w_n}
x^n+1,nx^n+1,nx^n+1,n=FFFx^n,nx^n,nx^n,n+GunGunGun+wnwnwn其中:
x
^
n
+
1
,
n
\pmb{\hat{x}_{n+1,n}}
x^n+1,nx^n+1,nx^n+1,n 是在时间点
n
+
1
n+1
n+1 的预测状态向量
x
^
n
,
n
\pmb{\hat{x}_{n,n}}
x^n,nx^n,nx^n,n 是在时间点
n
n
n 的估计状态向量
u
n
\pmb{u_n}
ununun 是控制变量
w
n
\pmb{w_n}
wnwnwn 是过程噪声
F
\pmb{F}
FFF 是状态过渡矩阵
G
\pmb{G}
GGG 是控制矩阵
在这里例子里状态外推方程可以简化为: x ^ n + 1 , n = F x ^ n , n \pmb{\hat{x}_{n+1,n}} = \pmb{F}\pmb{\hat{x}_{n,n}} x^n+1,nx^n+1,nx^n+1,n=FFFx^n,nx^n,nx^n,n
系统状态 x n x_n xn 为: x ^ n = [ x ^ n x ˙ ^ n x ¨ ^ n y ^ n y ˙ ^ n y ¨ ^ n ] \pmb{\hat{x}_n}=\begin{bmatrix} \hat{x}_n \\ \hat{\dot{x}}_n \\ \hat{\ddot{x}}_n \\ \hat{y}_n \\ \hat{\dot{y}}_n \\ \hat{\ddot{y}}_n \end{bmatrix} x^nx^nx^n=⎣⎢⎢⎢⎢⎢⎢⎡x^nx˙^nx¨^ny^ny˙^ny¨^n⎦⎥⎥⎥⎥⎥⎥⎤
到时间点 n + 1 n+1 n+1 的车子外推状态可以用以下公式描述:
{ x ^ n + 1 , n = x ^ n , n + x ˙ ^ n , n Δ t + 1 2 x ¨ n , n Δ t 2 x ˙ ^ n + 1 , n = x ˙ ^ n , n + x ¨ ^ n , n Δ t x ¨ ^ n + 1 , n = x ¨ ^ n , n y ^ n + 1 , n = y ^ n , n + y ˙ ^ n , n Δ t + 1 2 y ¨ n , n Δ t 2 y ˙ ^ n + 1 , n = y ˙ ^ n , n + y ¨ ^ n , n Δ t y ¨ ^ n + 1 , n = y ¨ ^ n , n \left\{ \begin{aligned} \hat{x}_{n+1,n} &= \hat{x}_{n,n} + \hat{\dot{x}}_{n,n} \Delta t + \frac{1}{2}\ddot{x}_{n,n} \Delta t^2 \\ \hat{\dot{x}}_{n+1,n} &= \hat{\dot{x}}_{n,n} + \hat{\ddot{x}}_{n,n} \Delta t \\ \hat{\ddot{x}}_{n+1,n} &= \hat{\ddot{x}}_{n,n} \\ \hat{y}_{n+1,n} &= \hat{y}_{n,n} + \hat{\dot{y}}_{n,n} \Delta t + \frac{1}{2}\ddot{y}_{n,n} \Delta t^2 \\ \hat{\dot{y}}_{n+1,n} &= \hat{\dot{y}}_{n,n} + \hat{\ddot{y}}_{n,n} \Delta t \\ \hat{\ddot{y}}_{n+1,n} &= \hat{\ddot{y}}_{n,n} \end{aligned} \right. ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧x^n+1,nx˙^n+1,nx¨^n+1,ny^n+1,ny˙^n+1,ny¨^n+1,n=x^n,n+x˙^n,nΔt+21x¨n,nΔt2=x˙^n,n+x¨^n,nΔt=x¨^n,n=y^n,n+y˙^n,nΔt+21y¨n,nΔt2=y˙^n,n+y¨^n,nΔt=y¨^n,n
矩阵形式:
[
x
^
n
+
1
,
n
x
˙
^
n
+
1
,
n
x
¨
^
n
+
1
,
n
y
^
n
+
1
,
n
y
˙
^
n
+
1
,
n
y
¨
^
n
+
1
,
n
]
=
[
1
Δ
t
0.5
Δ
t
2
0
0
0
0
1
Δ
t
0
0
0
0
0
1
0
0
0
0
0
0
1
Δ
t
0.5
Δ
t
2
0
0
0
0
1
Δ
t
0
0
0
0
0
1
]
[
x
^
n
,
n
x
˙
^
n
,
n
x
¨
^
n
,
n
y
^
n
,
n
y
˙
^
n
,
n
y
¨
^
n
,
n
]
\begin{bmatrix} \hat{x}_{n+1,n} \\ \hat{\dot{x}}_{n+1,n} \\ \hat{\ddot{x}}_{n+1,n} \\ \hat{y}_{n+1,n} \\ \hat{\dot{y}}_{n+1,n} \\ \hat{\ddot{y}}_{n+1,n} \\ \end{bmatrix}= \begin{bmatrix} 1 & \Delta t & 0.5\Delta t^2 & 0 & 0 & 0 \\ 0 & 1 & \Delta t & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & \Delta t & 0.5\Delta t^2 \\ 0 & 0 & 0 & 0 & 1 & \Delta t \\ 0 & 0 & 0 & 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} \hat{x}_{n,n} \\ \hat{\dot{x}}_{n,n} \\ \hat{\ddot{x}}_{n,n} \\ \hat{y}_{n,n} \\ \hat{\dot{y}}_{n,n} \\ \hat{\ddot{y}}_{n,n} \end{bmatrix}
⎣⎢⎢⎢⎢⎢⎢⎡x^n+1,nx˙^n+1,nx¨^n+1,ny^n+1,ny˙^n+1,ny¨^n+1,n⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎡100000Δt100000.5Δt2Δt1000000100000Δt100000.5Δt2Δt1⎦⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎡x^n,nx˙^n,nx¨^n,ny^n,ny˙^n,ny¨^n,n⎦⎥⎥⎥⎥⎥⎥⎤
协方差外推方程
协方差外推方程的一般形式为:
P
n
+
1
,
n
=
F
P
n
,
n
F
T
+
Q
\pmb{P_{n+1,n} = FP_{n,n}F^T + Q}
Pn+1,n=FPn,nFT+QPn+1,n=FPn,nFT+QPn+1,n=FPn,nFT+Q其中:
P
n
,
n
\pmb{P_{n,n}}
Pn,nPn,nPn,n 是当前状态的估计不确定性(协方差)矩阵
P
n
+
1
,
n
\pmb{P_{n+1,n}}
Pn+1,nPn+1,nPn+1,n 是在当前预测的下一个状态的估计不确定性(协方差)矩阵
F
\pmb{F}
FFF 是之前在“线性动态系统模型”部分推导出来的状态转移矩阵
Q
\pmb{Q}
QQQ 是过程噪声矩阵
估计不确定性的矩阵形式为: P = [ p x p x x ˙ p x x ¨ p x y p x y ˙ p x y ¨ p x ˙ x p x ˙ p x ˙ x ¨ p x ˙ y p x ˙ y ˙ p x ˙ y ¨ p x ¨ x p x ¨ x ˙ p x ¨ p x ¨ y p x ¨ y ˙ p x ¨ y ¨ p y x p y x ˙ p y x ¨ p y p y y ˙ p y y ¨ p y ˙ x p y ˙ x ˙ p y ˙ x ¨ p y ˙ y p y ˙ p y ˙ y ¨ p y ¨ x p y ¨ x ˙ p y ¨ x ¨ p y ¨ y p y ¨ y ˙ p y ¨ ] \pmb{P} = \begin{bmatrix} p_x & p_{x\dot{x}} & p_{x\ddot{x}} & p_{xy} & p_{x\dot{y}} & p_{x\ddot{y}} \\ p_{\dot{x}x} & p_{\dot{x}} & p_{\dot{x}\ddot{x}} & p_{\dot{x}y} & p_{\dot{x}\dot{y}} & p_{\dot{x}\ddot{y}} \\ p_{\ddot{x}x} & p_{\ddot{x}\dot{x}} & p_{\ddot{x}} & p_{\ddot{x}y} & p_{\ddot{x}\dot{y}} & p_{\ddot{x}\ddot{y}} \\ p_{yx} & p_{y\dot{x}} & p_{y\ddot{x}} & p_{y} & p_{y\dot{y}} & p_{y\ddot{y}} \\ p_{\dot{y}x} & p_{\dot{y}\dot{x}} & p_{\dot{y}\ddot{x}} & p_{\dot{y}y} & p_{\dot{y}} & p_{\dot{y}\ddot{y}} \\ p_{\ddot{y}x} & p_{\ddot{y}\dot{x}} & p_{\ddot{y}\ddot{x}} & p_{\ddot{y}y} & p_{\ddot{y}\dot{y}} & p_{\ddot{y}} \\ \end{bmatrix} PPP=⎣⎢⎢⎢⎢⎢⎢⎡pxpx˙xpx¨xpyxpy˙xpy¨xpxx˙px˙px¨x˙pyx˙py˙x˙py¨x˙pxx¨px˙x¨px¨pyx¨py˙x¨py¨x¨pxypx˙ypx¨ypypy˙ypy¨ypxy˙px˙y˙px¨y˙pyy˙py˙py¨y˙pxy¨px˙y¨px¨y¨pyy¨py˙y¨py¨⎦⎥⎥⎥⎥⎥⎥⎤
矩阵的对角线是估计的方差:
∙
\bullet
∙
p
x
p_x
px 是
X
X
X 轴位置估计的方差
∙
\bullet
∙
p
x
˙
p_{\dot{x}}
px˙ 是
X
X
X 轴速度估计的方差
∙
\bullet
∙
p
x
¨
p_{\ddot{x}}
px¨ 是
X
X
X 轴加速度估计的方差
∙
\bullet
∙
p
y
p_{y}
py 是
Y
Y
Y 轴位置估计的方差
∙
\bullet
∙
p
y
˙
p_{\dot{y}}
py˙ 是
Y
Y
Y 轴速度估计的方差
∙
\bullet
∙
p
y
¨
p_{\ddot{y}}
py¨ 是
Y
Y
Y 轴加速度估计的方差
∙
\bullet
∙ 非对角线上的是协方差
假设 X X X 和 Y Y Y 轴上的估计误差(estimation errors)不相关,因此可以将相互项设置为零. P = [ p x p x x ˙ p x x ¨ 0 0 0 p x ˙ x p x ˙ p x ˙ x ¨ 0 0 0 p x ¨ x p x ¨ x ˙ p x ¨ 0 0 0 0 0 0 p y p y y ˙ p y y ¨ 0 0 0 p y ˙ y p y ˙ p y ˙ y ¨ 0 0 0 p y ¨ y p y ¨ y ˙ p y ¨ ] \pmb{P} = \begin{bmatrix} p_x & p_{x\dot{x}} & p_{x\ddot{x}} & 0 & 0 & 0 \\ p_{\dot{x}x} & p_{\dot{x}} & p_{\dot{x}\ddot{x}} & 0 & 0 & 0 \\ p_{\ddot{x}x} & p_{\ddot{x}\dot{x}} & p_{\ddot{x}} & 0 & 0 & 0 \\ 0 & 0 & 0 & p_{y} & p_{y\dot{y}} & p_{y\ddot{y}} \\ 0 & 0 & 0 & p_{\dot{y}y} & p_{\dot{y}} & p_{\dot{y}\ddot{y}} \\ 0 & 0 & 0 & p_{\ddot{y}y} & p_{\ddot{y}\dot{y}} & p_{\ddot{y}} \\ \end{bmatrix} PPP=⎣⎢⎢⎢⎢⎢⎢⎡pxpx˙xpx¨x000pxx˙px˙px¨x˙000pxx¨px˙x¨px¨000000pypy˙ypy¨y000pyy˙py˙py¨y˙000pyy¨py˙y¨py¨⎦⎥⎥⎥⎥⎥⎥⎤
过程噪声矩阵
假设是离散噪声模型:不同时间片的噪声是不同的,但是在单个时间片内是不变的。
二维恒定加速度模型的过程噪声矩阵如下所示:
Q
=
[
σ
x
2
σ
x
x
˙
2
σ
x
x
¨
2
σ
x
y
2
σ
x
y
˙
2
σ
x
y
¨
2
σ
x
˙
x
2
σ
x
˙
2
σ
x
˙
x
¨
2
σ
x
˙
y
2
σ
x
˙
y
˙
2
σ
x
˙
y
¨
2
σ
x
¨
x
2
σ
x
¨
x
˙
2
σ
x
¨
2
σ
x
¨
y
2
σ
x
¨
y
˙
2
σ
x
¨
y
¨
2
σ
y
x
2
σ
y
x
˙
2
σ
y
x
¨
2
σ
y
2
σ
y
y
˙
2
σ
y
y
¨
2
σ
y
˙
x
2
σ
y
˙
x
˙
2
σ
y
˙
x
¨
2
σ
y
˙
y
2
σ
y
˙
2
σ
y
˙
y
¨
2
σ
y
¨
x
2
σ
y
¨
x
˙
2
σ
y
¨
x
¨
2
σ
y
¨
y
2
σ
y
¨
y
˙
2
σ
y
¨
2
]
\pmb{Q} = \begin{bmatrix} \sigma^2_{x} & \sigma^2_{x\dot{x}} & \sigma^2_{x\ddot{x}} & \sigma^2_{xy} & \sigma^2_{x\dot{y}} & \sigma^2_{x\ddot{y}} \\[0.5em] \sigma^2_{\dot{x}x} & \sigma^2_{\dot{x}} & \sigma^2_{\dot{x}\ddot{x}} & \sigma^2_{\dot{x}y} & \sigma^2_{\dot{x}\dot{y}} & \sigma^2_{\dot{x}\ddot{y}} \\[0.5em] \sigma^2_{\ddot{x}x} & \sigma^2_{\ddot{x}\dot{x}} & \sigma^2_{\ddot{x}} & \sigma^2_{\ddot{x}y} & \sigma^2_{\ddot{x}\dot{y}} & \sigma^2_{\ddot{x}\ddot{y}} \\[0.5em] \sigma^2_{yx} & \sigma^2_{y\dot{x}} & \sigma^2_{y\ddot{x}} & \sigma^2_{y} & \sigma^2_{y\dot{y}} & \sigma^2_{y\ddot{y}} \\[0.5em] \sigma^2_{\dot{y}x} & \sigma^2_{\dot{y}\dot{x}} & \sigma^2_{\dot{y}\ddot{x}} & \sigma^2_{\dot{y}y} & \sigma^2_{\dot{y}} & \sigma^2_{\dot{y}\ddot{y}} \\[0.5em] \sigma^2_{\ddot{y}x} & \sigma^2_{\ddot{y}\dot{x}} & \sigma^2_{\ddot{y}\ddot{x}} & \sigma^2_{\ddot{y}y} & \sigma^2_{\ddot{y}\dot{y}} & \sigma^2_{\ddot{y}} \end{bmatrix}
QQQ=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡σx2σx˙x2σx¨x2σyx2σy˙x2σy¨x2σxx˙2σx˙2σx¨x˙2σyx˙2σy˙x˙2σy¨x˙2σxx¨2σx˙x¨2σx¨2σyx¨2σy˙x¨2σy¨x¨2σxy2σx˙y2σx¨y2σy2σy˙y2σy¨y2σxy˙2σx˙y˙2σx¨y˙2σyy˙2σy˙2σy¨y˙2σxy¨2σx˙y¨2σx¨y¨2σyy¨2σy˙y¨2σy¨2⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
假设 X X X 轴和 Y Y Y 轴的过程噪声是不相关的,则相互项设为 0 0 0: Q = [ σ x 2 σ x x ˙ 2 σ x x ¨ 2 0 0 0 σ x ˙ x 2 σ x ˙ 2 σ x ˙ x ¨ 2 0 0 0 σ x ¨ x 2 σ x ¨ x ˙ 2 σ x ¨ 2 0 0 0 0 0 0 σ y 2 σ y y ˙ 2 σ y y ¨ 2 0 0 0 σ y ˙ y 2 σ y ˙ 2 σ y ˙ y ¨ 2 0 0 0 σ y ¨ y 2 σ y ¨ y ˙ 2 σ y ¨ 2 ] \pmb{Q} = \begin{bmatrix} \sigma^2_{x} & \sigma^2_{x\dot{x}} & \sigma^2_{x\ddot{x}} & 0 & 0 & 0 \\[0.5em] \sigma^2_{\dot{x}x} & \sigma^2_{\dot{x}} & \sigma^2_{\dot{x}\ddot{x}} & 0 & 0 & 0 \\[0.5em] \sigma^2_{\ddot{x}x} & \sigma^2_{\ddot{x}\dot{x}} & \sigma^2_{\ddot{x}} & 0 & 0 & 0 \\[0.5em] 0 & 0 & 0 & \sigma^2_{y} & \sigma^2_{y\dot{y}} & \sigma^2_{y\ddot{y}} \\[0.5em] 0 & 0 & 0 & \sigma^2_{\dot{y}y} & \sigma^2_{\dot{y}} & \sigma^2_{\dot{y}\ddot{y}} \\[0.5em] 0 & 0 & 0 & \sigma^2_{\ddot{y}y} & \sigma^2_{\ddot{y}\dot{y}} & \sigma^2_{\ddot{y}} \end{bmatrix} QQQ=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡σx2σx˙x2σx¨x2000σxx˙2σx˙2σx¨x˙2000σxx¨2σx˙x¨2σx¨2000000σy2σy˙y2σy¨y2000σyy˙2σy˙2σy¨y˙2000σyy¨2σy˙y¨2σy¨2⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
前面已经推导了恒定加速度模型的 Q 矩阵,对于我们这个例子,Q 矩阵为:
Q
=
[
Δ
t
4
4
Δ
t
3
2
Δ
t
2
2
0
0
0
Δ
t
3
2
Δ
t
2
Δ
t
0
0
0
Δ
t
2
2
Δ
t
1
0
0
0
0
0
0
Δ
t
4
4
Δ
t
3
2
Δ
t
2
2
0
0
0
Δ
t
3
2
Δ
t
2
Δ
t
0
0
0
Δ
t
2
2
Δ
t
1
]
σ
a
2
\pmb{Q} = \begin{bmatrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} & \frac{\Delta t^2}{2} & 0 & 0 & 0 \\[0.5em] \frac{\Delta t^3}{2} & \Delta t^2 & \Delta t & 0 & 0 & 0 \\[0.5em] \frac{\Delta t^2}{2} & \Delta t & 1 & 0 & 0 & 0 \\[0.5em] 0 & 0 & 0 &\frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} & \frac{\Delta t^2}{2} \\[0.5em] 0 & 0 & 0 & \frac{\Delta t^3}{2} & \Delta t^2 & \Delta t \\[0.5em] 0 & 0 & 0 & \frac{\Delta t^2}{2} & \Delta t & 1 \end{bmatrix} \sigma^2_{a}
QQQ=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡4Δt42Δt32Δt20002Δt3Δt2Δt0002Δt2Δt10000004Δt42Δt32Δt20002Δt3Δt2Δt0002Δt2Δt1⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤σa2其中:
∙
\bullet
∙
Δ
t
\Delta t
Δt 是连续测量之间的时间间隔
∙
\bullet
∙
σ
a
2
\sigma^2_a
σa2 是加速度中的随机方差
下面给出本例子的协方差外推方程:
P
n
+
1
,
n
=
F
P
n
,
n
F
T
+
Q
\pmb{P_{n+1,n} = FP_{n,n}F^T + Q}
Pn+1,n=FPn,nFT+QPn+1,n=FPn,nFT+QPn+1,n=FPn,nFT+Q
[
p
x
n
+
1
,
n
p
x
x
˙
n
+
1
,
n
p
x
x
¨
n
+
1
,
n
0
0
0
p
x
˙
x
n
+
1
,
n
p
x
˙
n
+
1
,
n
p
x
˙
x
¨
n
+
1
,
n
0
0
0
p
x
¨
x
n
+
1
,
n
p
x
¨
x
˙
n
+
1
,
n
p
x
¨
n
+
1
,
n
0
0
0
0
0
0
p
y
n
+
1
,
n
p
y
y
˙
n
+
1
,
n
p
y
y
¨
n
+
1
,
n
0
0
0
p
y
˙
y
n
+
1
,
n
p
y
˙
n
+
1
,
n
p
y
˙
y
¨
n
+
1
,
n
0
0
0
p
y
¨
y
n
+
1
,
n
p
y
¨
y
˙
n
+
1
,
n
p
y
¨
n
+
1
,
n
]
=
[
1
Δ
t
0.5
Δ
t
2
0
0
0
0
1
Δ
t
0
0
0
0
0
1
0
0
0
0
0
0
1
Δ
t
0.5
Δ
t
2
0
0
0
0
1
Δ
t
0
0
0
0
0
1
]
×
[
p
x
n
,
n
p
x
x
˙
n
,
n
p
x
x
¨
n
,
n
0
0
0
p
x
˙
x
n
,
n
p
x
˙
n
,
n
p
x
˙
x
¨
n
,
n
0
0
0
p
x
¨
x
n
,
n
p
x
¨
x
˙
n
,
n
p
x
¨
n
,
n
0
0
0
0
0
0
p
y
n
,
n
p
y
y
˙
n
,
n
p
y
y
¨
n
,
n
0
0
0
p
y
˙
y
n
,
n
p
y
˙
n
,
n
p
y
˙
y
¨
n
,
n
0
0
0
p
y
¨
y
n
,
n
p
y
¨
y
˙
n
,
n
p
y
¨
n
,
n
]
×
[
1
0
0
0
0
0
Δ
t
1
0
0
0
0
0.5
Δ
t
2
Δ
t
1
0
0
0
0
0
0
1
0
0
0
0
0
Δ
t
1
0
0
0
0
0.5
Δ
t
2
Δ
t
1
]
+
[
Δ
t
4
4
Δ
t
3
2
Δ
t
2
2
0
0
0
Δ
t
3
2
Δ
t
2
Δ
t
0
0
0
Δ
t
2
2
Δ
t
1
0
0
0
0
0
0
Δ
t
4
4
Δ
t
3
2
Δ
t
2
2
0
0
0
Δ
t
3
2
Δ
t
2
Δ
t
0
0
0
Δ
t
2
2
Δ
t
1
]
σ
a
2
\begin{bmatrix} p_{x_{n+1,n}} & p_{x\dot{x}_{n+1,n}} & p_{x\ddot{x}_{n+1,n}} & 0 & 0 & 0 \\ p_{\dot{x}x_{n+1,n}} & p_{\dot{x}_{n+1,n}} & p_{\dot{x}\ddot{x}_{n+1,n}} & 0 & 0 & 0 \\ p_{\ddot{x}x_{n+1,n}} & p_{\ddot{x}\dot{x}_{n+1,n}} & p_{\ddot{x}_{n+1,n}} & 0 & 0 & 0 \\ 0 & 0 & 0 & p_{y_{n+1,n}} & p_{y\dot{y}_{n+1,n}} & p_{y\ddot{y}_{n+1,n}} \\ 0 & 0 & 0 & p_{\dot{y}y_{n+1,n}} & p_{\dot{y}_{n+1,n}} & p_{\dot{y}\ddot{y}_{n+1,n}} \\ 0 & 0 & 0 & p_{\ddot{y}y_{n+1,n}} & p_{\ddot{y}\dot{y}_{n+1,n}} & p_{\ddot{y}_{n+1,n}} \\ \end{bmatrix} = \begin{bmatrix} 1 & \Delta t & 0.5\Delta t^2 & 0 & 0 & 0 \\ 0 & 1 & \Delta t & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & \Delta t & 0.5\Delta t^2 \\ 0 & 0 & 0 & 0 & 1 & \Delta t \\ 0 & 0 & 0 & 0 & 0 & 1 \\ \end{bmatrix} \times \begin{bmatrix} p_{x_{n,n}} & p_{x\dot{x}_{n,n}} & p_{x\ddot{x}_{n,n}} & 0 & 0 & 0 \\ p_{\dot{x}x_{n,n}} & p_{\dot{x}_{n,n}} & p_{\dot{x}\ddot{x}_{n,n}} & 0 & 0 & 0 \\ p_{\ddot{x}x_{n,n}} & p_{\ddot{x}\dot{x}_{n,n}} & p_{\ddot{x}_{n,n}} & 0 & 0 & 0 \\ 0 & 0 & 0 & p_{y_{n,n}} & p_{y\dot{y}_{n,n}} & p_{y\ddot{y}_{n,n}} \\ 0 & 0 & 0 & p_{\dot{y}y_{n,n}} & p_{\dot{y}_{n,n}} & p_{\dot{y}\ddot{y}_{n,n}} \\ 0 & 0 & 0 & p_{\ddot{y}y_{n,n}} & p_{\ddot{y}\dot{y}_{n,n}} & p_{\ddot{y}_{n,n}} \\ \end{bmatrix} \times \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ \Delta t & 1 & 0 & 0 & 0 & 0 \\ 0.5\Delta t^2 & \Delta t & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & \Delta t & 1 & 0 \\ 0 & 0 & 0 & 0.5\Delta t^2 & \Delta t & 1 \\ \end{bmatrix} + \begin{bmatrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} & \frac{\Delta t^2}{2} & 0 & 0 & 0 \\[0.5em] \frac{\Delta t^3}{2} & \Delta t^2 & \Delta t & 0 & 0 & 0 \\[0.5em] \frac{\Delta t^2}{2} & \Delta t & 1 & 0 & 0 & 0 \\[0.5em] 0 & 0 & 0 &\frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} & \frac{\Delta t^2}{2} \\[0.5em] 0 & 0 & 0 & \frac{\Delta t^3}{2} & \Delta t^2 & \Delta t \\[0.5em] 0 & 0 & 0 & \frac{\Delta t^2}{2} & \Delta t & 1 \end{bmatrix} \sigma^2_{a}
⎣⎢⎢⎢⎢⎢⎢⎡pxn+1,npx˙xn+1,npx¨xn+1,n000pxx˙n+1,npx˙n+1,npx¨x˙n+1,n000pxx¨n+1,npx˙x¨n+1,npx¨n+1,n000000pyn+1,npy˙yn+1,npy¨yn+1,n000pyy˙n+1,npy˙n+1,npy¨y˙n+1,n000pyy¨n+1,npy˙y¨n+1,npy¨n+1,n⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎡100000Δt100000.5Δt2Δt1000000100000Δt100000.5Δt2Δt1⎦⎥⎥⎥⎥⎥⎥⎤×⎣⎢⎢⎢⎢⎢⎢⎡pxn,npx˙xn,npx¨xn,n000pxx˙n,npx˙n,npx¨x˙n,n000pxx¨n,npx˙x¨n,npx¨n,n000000pyn,npy˙yn,npy¨yn,n000pyy˙n,npy˙n,npy¨y˙n,n000pyy¨n,npy˙y¨n,npy¨n,n⎦⎥⎥⎥⎥⎥⎥⎤×⎣⎢⎢⎢⎢⎢⎢⎡1Δt0.5Δt200001Δt0000010000001Δt0.5Δt200001Δt000001⎦⎥⎥⎥⎥⎥⎥⎤+⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡4Δt42Δt32Δt20002Δt3Δt2Δt0002Δt2Δt10000004Δt42Δt32Δt20002Δt3Δt2Δt0002Δt2Δt1⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤σa2
测量方程
矩阵形式的测量方程的一般形式为:
z
n
=
H
x
n
+
v
n
\pmb{z_n} = \pmb{H}\pmb{x_n} + \pmb{v_n}
znznzn=HHHxnxnxn+vnvnvn其中:
z
n
\pmb{z_n}
znznzn 是测量向量(measurement vector)
x
n
\pmb{x_n}
xnxnxn 是真实系统状态(隐藏状态)
v
n
\pmb{v_n}
vnvnvn 是随机噪声向量
H
\pmb{H}
HHH 是观察矩阵(observation matrix)
这里只提供了车子 X X X 轴和 Y Y Y 轴的测量值,因此 z n = [ x n , m e a s u r e d y n , m e a s u r e d ] \pmb{z_n} = \begin{bmatrix} x_{n,measured} \\ y_{n,measured} \end{bmatrix} znznzn=[xn,measuredyn,measured]
z n = H x n [ x n , m e a s u r e d y n , m e a s u r e d ] = H [ x n x ˙ n x ¨ n y n y ˙ n y ¨ n ] \begin{aligned} \pmb{z_n} &= \pmb{H}\pmb{x_n} \\ \begin{bmatrix} x_{n,measured} \\ y_{n,measured} \end{bmatrix} &= \pmb{H} \begin{bmatrix} x_n \\ \dot{x}_n \\ \ddot{x}_n \\ y_n \\ \dot{y}_n \\ \ddot{y}_n \end{bmatrix} \end{aligned} znznzn[xn,measuredyn,measured]=HHHxnxnxn=HHH⎣⎢⎢⎢⎢⎢⎢⎡xnx˙nx¨nyny˙ny¨n⎦⎥⎥⎥⎥⎥⎥⎤
z n \pmb{z_n} znznzn 的维度是 2 × 1 2 \times 1 2×1, x n \pmb{x_n} xnxnxn 的维度是 6 × 1 6 \times 1 6×1,因此观察矩阵 H \pmb{H} HHH 的维度是 2 × 6 2 \times 6 2×6 H = [ 1 0 0 0 0 0 0 0 0 1 0 0 ] \pmb{H} = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \end{bmatrix} HHH=[100000010000]
测量不确定性
测量协方差矩阵为: R n = [ σ x m 2 σ y x m 2 σ x y m 2 σ y m 2 ] \pmb{R_n} = \begin{bmatrix} \sigma^2_{x_m} & \sigma^2_{yx_m} \\[0.5em] \sigma^2_{xy_m} & \sigma^2_{y_m} \end{bmatrix} RnRnRn=[σxm2σxym2σyxm2σym2]
下标 m m m 是测量不确定性
假设测量值 x x x 和 y y y 是不相关的,即 x x x 坐标测量值的误差不取决于 y y y 坐标测量值的误差。 R n = [ σ x m 2 0 0 σ y m 2 ] \pmb{R_n} = \begin{bmatrix} \sigma^2_{x_m} & 0 \\[0.5em] 0 & \sigma^2_{y_m} \end{bmatrix} RnRnRn=[σxm200σym2]
在实际应用中,测量不确定度可能因测量不同而不同。在许多系统中,测量不确定度取决于测量信噪比、传感器与目标的夹角、信号频率等参数。
为了让例子更简单,我们将假设一个恒定的测量不确定性: R 1 = R 2 … R n − 1 = R n = R \pmb{R_1} = \pmb{R_2} \dots \pmb{R_{n-1}} = \pmb{R_n} = \pmb{R} R1R1R1=R2R2R2…Rn−1Rn−1Rn−1=RnRnRn=RRR
卡尔曼增益
矩阵形式的卡尔曼增益如下:
K
n
=
P
n
,
n
−
1
H
T
(
H
P
n
,
n
−
1
H
T
+
R
n
)
−
1
\pmb{K_n} = \pmb{P_{n,n-1}} \pmb{H}^T (\pmb{HP_{n,n-1}H}^T + \pmb{R_n})^{-1}
KnKnKn=Pn,n−1Pn,n−1Pn,n−1HHHT(HPn,n−1HHPn,n−1HHPn,n−1HT+RnRnRn)−1其中:
K
n
\pmb{K_n}
KnKnKn 是卡尔曼增益
P
n
,
n
−
1
\pmb{P_{n,n-1}}
Pn,n−1Pn,n−1Pn,n−1 是在当前时刻的先前估计不确定性(方差)矩阵(在上一个状态预测得到的)
H
\pmb{H}
HHH 是观察矩阵
R
n
\pmb{R_n}
RnRnRn 是测量不确定性(测量噪声协方差矩阵)
[ K 1 , 1 n K 1 , 2 n K 2 , 1 n K 2 , 2 n K 3 , 1 n K 3 , 2 n K 4 , 1 n K 4 , 2 n K 5 , 1 n K 5 , 2 n K 6 , 1 n K 6 , 2 n ] = [ p x n , n − 1 p x x ˙ n , n − 1 p x x ¨ n , n − 1 0 0 0 p x ˙ x n , n − 1 p x ˙ n , n − 1 p x ˙ x ¨ n , n − 1 0 0 0 p x ¨ x n , n − 1 p x ¨ x ˙ n , n − 1 p x ¨ n , n − 1 0 0 0 0 0 0 p y n , n − 1 p y y ˙ n , n − 1 p y y ¨ n , n − 1 0 0 0 p y ˙ y n , n − 1 p y ˙ n , n − 1 p y ˙ y ¨ n , n − 1 0 0 0 p y ¨ y n , n − 1 p y ¨ y ˙ n , n − 1 p y ¨ n , n − 1 ] [ 1 0 0 0 0 0 0 1 0 0 0 0 ] × ( [ 1 0 0 0 0 0 0 0 0 1 0 0 ] [ p x n , n − 1 p x x ˙ n , n − 1 p x x ¨ n , n − 1 0 0 0 p x ˙ x n , n − 1 p x ˙ n , n − 1 p x ˙ x ¨ n , n − 1 0 0 0 p x ¨ x n , n − 1 p x ¨ x ˙ n , n − 1 p x ¨ n , n − 1 0 0 0 0 0 0 p y n , n − 1 p y y ˙ n , n − 1 p y y ¨ n , n − 1 0 0 0 p y ˙ y n , n − 1 p y ˙ n , n − 1 p y ˙ y ¨ n , n − 1 0 0 0 p y ¨ y n , n − 1 p y ¨ y ˙ n , n − 1 p y ¨ n , n − 1 ] [ 1 0 0 0 0 0 0 1 0 0 0 0 ] + [ σ x m 2 0 0 σ y m 2 ] ) − 1 \begin{bmatrix} K_{1,1_n} & K_{1,2_n} \\ K_{2,1_n} & K_{2,2_n} \\ K_{3,1_n} & K_{3,2_n} \\ K_{4,1_n} & K_{4,2_n} \\ K_{5,1_n} & K_{5,2_n} \\ K_{6,1_n} & K_{6,2_n} \\ \end{bmatrix} = \begin{bmatrix} p_{x_{n,n-1}} & p_{x\dot{x}_{n,n-1}} & p_{x\ddot{x}_{n,n-1}} & 0 & 0 & 0 \\ p_{\dot{x}x_{n,n-1}} & p_{\dot{x}_{n,n-1}} & p_{\dot{x}\ddot{x}_{n,n-1}} & 0 & 0 & 0 \\ p_{\ddot{x}x_{n,n-1}} & p_{\ddot{x}\dot{x}_{n,n-1}} & p_{\ddot{x}_{n,n-1}} & 0 & 0 & 0 \\ 0 & 0 & 0 & p_{y_{n,n-1}} & p_{y\dot{y}_{n,n-1}} & p_{y\ddot{y}_{n,n-1}} \\ 0 & 0 & 0 & p_{\dot{y}y_{n,n-1}} & p_{\dot{y}_{n,n-1}} & p_{\dot{y}\ddot{y}_{n,n-1}} \\ 0 & 0 & 0 & p_{\ddot{y}y_{n,n-1}} & p_{\ddot{y}\dot{y}_{n,n-1}} & p_{\ddot{y}_{n,n-1}} \\ \end{bmatrix} \begin{bmatrix} 1 & 0 \\ 0 & 0 \\ 0 & 0 \\ 0 & 1 \\ 0 & 0 \\ 0 & 0 \\ \end{bmatrix} \times \left( \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \end{bmatrix} \begin{bmatrix} p_{x_{n,n-1}} & p_{x\dot{x}_{n,n-1}} & p_{x\ddot{x}_{n,n-1}} & 0 & 0 & 0 \\ p_{\dot{x}x_{n,n-1}} & p_{\dot{x}_{n,n-1}} & p_{\dot{x}\ddot{x}_{n,n-1}} & 0 & 0 & 0 \\ p_{\ddot{x}x_{n,n-1}} & p_{\ddot{x}\dot{x}_{n,n-1}} & p_{\ddot{x}_{n,n-1}} & 0 & 0 & 0 \\ 0 & 0 & 0 & p_{y_{n,n-1}} & p_{y\dot{y}_{n,n-1}} & p_{y\ddot{y}_{n,n-1}} \\ 0 & 0 & 0 & p_{\dot{y}y_{n,n-1}} & p_{\dot{y}_{n,n-1}} & p_{\dot{y}\ddot{y}_{n,n-1}} \\ 0 & 0 & 0 & p_{\ddot{y}y_{n,n-1}} & p_{\ddot{y}\dot{y}_{n,n-1}} & p_{\ddot{y}_{n,n-1}} \\ \end{bmatrix} \begin{bmatrix} 1 & 0 \\ 0 & 0 \\ 0 & 0 \\ 0 & 1 \\ 0 & 0 \\ 0 & 0 \\ \end{bmatrix} + \begin{bmatrix} \sigma^2_{x_m} & 0 \\[0.5em] 0 & \sigma^2_{y_m} \end{bmatrix} \right)^{-1} ⎣⎢⎢⎢⎢⎢⎢⎡K1,1nK2,1nK3,1nK4,1nK5,1nK6,1nK1,2nK2,2nK3,2nK4,2nK5,2nK6,2n⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎡pxn,n−1px˙xn,n−1px¨xn,n−1000pxx˙n,n−1px˙n,n−1px¨x˙n,n−1000pxx¨n,n−1px˙x¨n,n−1px¨n,n−1000000pyn,n−1py˙yn,n−1py¨yn,n−1000pyy˙n,n−1py˙n,n−1py¨y˙n,n−1000pyy¨n,n−1py˙y¨n,n−1py¨n,n−1⎦⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎡100000000100⎦⎥⎥⎥⎥⎥⎥⎤×⎝⎜⎜⎜⎜⎜⎜⎛[100000010000]⎣⎢⎢⎢⎢⎢⎢⎡pxn,n−1px˙xn,n−1px¨xn,n−1000pxx˙n,n−1px˙n,n−1px¨x˙n,n−1000pxx¨n,n−1px˙x¨n,n−1px¨n,n−1000000pyn,n−1py˙yn,n−1py¨yn,n−1000pyy˙n,n−1py˙n,n−1py¨y˙n,n−1000pyy¨n,n−1py˙y¨n,n−1py¨n,n−1⎦⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎡100000000100⎦⎥⎥⎥⎥⎥⎥⎤+[σxm200σym2]⎠⎟⎟⎟⎟⎟⎟⎞−1
状态更新方程
状态更新方程的矩阵形式为:
x
^
n
,
n
=
x
^
n
,
n
−
1
+
K
n
(
z
n
−
H
x
^
n
,
n
−
1
)
\pmb{\hat{x}_{n,n}} = \pmb{\hat{x}_{n,n-1}} + \pmb{K_n}(\pmb{z_n} - \pmb{H} \pmb{\hat{x}_{n,n-1}})
x^n,nx^n,nx^n,n=x^n,n−1x^n,n−1x^n,n−1+KnKnKn(znznzn−HHHx^n,n−1x^n,n−1x^n,n−1)其中:
x
^
n
,
n
\pmb{\hat{x}_{n,n}}
x^n,nx^n,nx^n,n 是在时刻
n
n
n 的估计系统状态向量
x
^
n
,
n
−
1
\pmb{\hat{x}_{n,n-1}}
x^n,n−1x^n,n−1x^n,n−1 是在时刻
n
−
1
n-1
n−1 的预测系统状态向量
K
n
\pmb{K_n}
KnKnKn 是卡尔曼增益
z
n
\pmb{z_n}
znznzn 是测量值
H
\pmb{H}
HHH 是观察矩阵(observation matrix)
构成状态更新方程的元素已经在前面全部定义好了。
协方差更新方程
P
n
,
n
=
(
I
−
K
n
H
)
P
n
,
n
−
1
(
I
−
K
n
H
)
T
+
K
n
R
n
K
n
T
\pmb{P_{n,n}} = (\pmb{I}-\pmb{K_nH}) \pmb{P_{n,n-1}} (\pmb{I} - \pmb{K_nH})^T + \pmb{K_n}\pmb{R_n}\pmb{K_n}^T
Pn,nPn,nPn,n=(III−KnHKnHKnH)Pn,n−1Pn,n−1Pn,n−1(III−KnHKnHKnH)T+KnKnKnRnRnRnKnKnKnT 其中
P
n
,
n
\pmb{P_{n,n}}
Pn,nPn,nPn,n 是当前状态的估计不确定性(协方差)矩阵
P
n
,
n
−
1
\pmb{P_{n,n-1}}
Pn,n−1Pn,n−1Pn,n−1 是在当前状态下,先前估计不确定性(协方差)矩阵 (在上一个状态时预测得到的)
K
n
\pmb{K_n}
KnKnKn 是卡尔曼增益
H
\pmb{H}
HHH 是观察矩阵
R
n
\pmb{R_n}
RnRnRn 是测量不确定性(测量噪声协方差矩阵)
构成协方差更新方程的所有元素已经在前面全部定义好了。
数值实例
假设车子以恒定的速度沿着 X X X 轴的方向直线行驶。跑了 300 300 300 米后向右转,转弯半径为 300 300 300 米。转弯的时候由于圆周运动而受到加速度(角加速度)。
车子的运动如图所示:
∙
\bullet
∙ 测量时间间隔:
Δ
t
=
1
s
\Delta t =1s
Δt=1s
∙
\bullet
∙ 随机加速度标准差:
σ
a
2
=
0.15
m
s
2
\sigma^2_a = 0.15 \dfrac{m}{s^2}
σa2=0.15s2m
∙
\bullet
∙ 测量误差标准差:
σ
x
m
=
σ
y
m
=
3
m
\sigma_{x_m} = \sigma_{y_m} = 3 m
σxm=σym=3m
∙
\bullet
∙ 状态过渡矩阵
F
F
F :
F
=
[
1
Δ
t
0.5
Δ
t
2
0
0
0
0
1
Δ
t
0
0
0
0
0
1
0
0
0
0
0
0
1
Δ
t
0.5
Δ
t
2
0
0
0
0
1
Δ
t
0
0
0
0
0
1
]
=
[
1
1
0.5
0
0
0
0
1
1
0
0
0
0
0
1
0
0
0
0
0
0
1
1
0.5
0
0
0
0
1
1
0
0
0
0
0
1
]
\pmb{F} = \begin{bmatrix} 1 & \Delta t & 0.5\Delta t^2 & 0 & 0 & 0 \\ 0 & 1 & \Delta t & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & \Delta t & 0.5\Delta t^2 \\ 0 & 0 & 0 & 0 & 1 & \Delta t \\ 0 & 0 & 0 & 0 & 0 & 1 \\ \end{bmatrix} = \begin{bmatrix} 1 & 1 & 0.5 & 0 & 0 & 0 \\ 0 & 1 & 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 1 & 0.5 \\ 0 & 0 & 0 & 0 & 1 & 1 \\ 0 & 0 & 0 & 0 & 0 & 1 \\ \end{bmatrix}
FFF=⎣⎢⎢⎢⎢⎢⎢⎡100000Δt100000.5Δt2Δt1000000100000Δt100000.5Δt2Δt1⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎡1000001100000.5110000001000001100000.511⎦⎥⎥⎥⎥⎥⎥⎤
∙
\bullet
∙ 过程噪声矩阵
Q
\pmb{Q}
QQQ:
Q
=
[
Δ
t
4
4
Δ
t
3
2
Δ
t
2
2
0
0
0
Δ
t
3
2
Δ
t
2
Δ
t
0
0
0
Δ
t
2
2
Δ
t
1
0
0
0
0
0
0
Δ
t
4
4
Δ
t
3
2
Δ
t
2
2
0
0
0
Δ
t
3
2
Δ
t
2
Δ
t
0
0
0
Δ
t
2
2
Δ
t
1
]
σ
a
2
=
[
1
4
1
2
1
2
0
0
0
1
2
1
1
0
0
0
1
2
1
1
0
0
0
0
0
0
1
4
1
2
1
2
0
0
0
1
2
1
1
0
0
0
1
2
1
1
]
0.1
5
2
\pmb{Q} = \begin{bmatrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} & \frac{\Delta t^2}{2} & 0 & 0 & 0 \\[0.5em] \frac{\Delta t^3}{2} & \Delta t^2 & \Delta t & 0 & 0 & 0 \\[0.5em] \frac{\Delta t^2}{2} & \Delta t & 1 & 0 & 0 & 0 \\[0.5em] 0 & 0 & 0 &\frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} & \frac{\Delta t^2}{2} \\[0.5em] 0 & 0 & 0 & \frac{\Delta t^3}{2} & \Delta t^2 & \Delta t \\[0.5em] 0 & 0 & 0 & \frac{\Delta t^2}{2} & \Delta t & 1 \end{bmatrix} \sigma^2_{a} = \begin{bmatrix} \frac{1}{4} & \frac{1}{2} & \frac{1}{2} & 0 & 0 & 0 \\[0.5em] \frac{1}{2} & 1 & 1 & 0 & 0 & 0 \\[0.5em] \frac{1}{2} & 1 & 1 & 0 & 0 & 0 \\[0.5em] 0 & 0 & 0 &\frac{1}{4} & \frac{1}{2} & \frac{1}{2} \\[0.5em] 0 & 0 & 0 & \frac{1}{2} & 1 & 1 \\[0.5em] 0 & 0 & 0 & \frac{1}{2} & 1 & 1 \end{bmatrix} 0.15^2
QQQ=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡4Δt42Δt32Δt20002Δt3Δt2Δt0002Δt2Δt10000004Δt42Δt32Δt20002Δt3Δt2Δt0002Δt2Δt1⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤σa2=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡4121210002111000211100000041212100021110002111⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤0.152
∙ \bullet ∙ 测量不确定性矩阵 R \pmb{R} RRR 为: R n = [ σ x m 2 0 0 σ y m 2 ] = [ 9 0 0 9 ] \pmb{R_n} = \begin{bmatrix} \sigma^2_{x_m} & 0 \\[0.5em] 0 & \sigma^2_{y_m} \end{bmatrix} = \begin{bmatrix} 9 & 0 \\ 0 & 9 \end{bmatrix} RnRnRn=[σxm200σym2]=[9009]
下面的表格记录了35次有噪声的测量:
1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | |
---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|
x(m) | -393.66 | -375.93 | -351.04 | -328.96 | -299.35 | -273.36 | -245.89 | -222.58 | -198.03 | -174.17 | -146.32 | -123.72 | -103.47 | -78.23 | -52.65 | -23.34 | 25.96 | 49.72 | 76.94 | 95.38 | 119.83 | 144.01 | 161.84 | 180.56 | 201.42 | 222.62 | 239.4 | 252.51 | 266.26 | 271.75 | 277.4 | 294.12 | 301.23 | 291.8 | 299.89 |
y(m) | 300.4 | 301.78 | 295.1 | 305.19 | 301.06 | 302.05 | 300 | 303.57 | 296.33 | 297.65 | 297.41 | 299.61 | 299.6 | 302.39 | 295.04 | 300.09 | 294.72 | 298.61 | 294.64 | 284.88 | 272.82 | 264.93 | 251.46 | 241.27 | 222.98 | 203.73 | 184.1 | 166.12 | 138.71 | 119.71 | 100.41 | 79.76 | 50.62 | 32.99 | 2.14 |
第 0 次迭代
初始化
我们不知道车子的位置;初始位置,车速,加速度都设为 0 0 0:
x ^ 0 , 0 = [ 0 0 0 0 0 0 ] \pmb{\hat{x}_{0,0}}=\begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0\end{bmatrix} x^0,0x^0,0x^0,0=⎣⎢⎢⎢⎢⎢⎢⎡000000⎦⎥⎥⎥⎥⎥⎥⎤
由于初始状态向量是个猜测值,我们将会设置很高的估计不确定性。高的估计不确定性会导致高卡尔曼增益,给到测量值很高的权重。 P 0 , 0 = [ 500 0 0 0 0 0 0 500 0 0 0 0 0 0 500 0 0 0 0 0 0 500 0 0 0 0 0 0 500 0 0 0 0 0 0 500 ] \pmb{P_{0,0}} = \begin{bmatrix} 500 & 0 & 0 & 0 & 0 & 0 \\ 0 & 500 & 0 & 0 & 0 & 0 \\ 0 & 0 & 500 & 0 & 0 & 0 \\ 0 & 0 & 0 & 500 & 0 & 0 \\ 0 & 0 & 0 & 0 & 500 & 0 \\ 0 & 0 & 0 & 0 & 0 & 500 \end{bmatrix} P0,0P0,0P0,0=⎣⎢⎢⎢⎢⎢⎢⎡500000000500000000500000000500000000500000000500⎦⎥⎥⎥⎥⎥⎥⎤
预测
现在我们可以基于初始值预测下一个状态: x ^ 1 , 0 = F x ^ 0 , 0 = [ 0 0 0 0 0 0 ] \pmb{\hat{x}_{1,0}} = \pmb{F} \pmb{\hat{x}_{0,0}} = \begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \end{bmatrix} x^1,0x^1,0x^1,0=FFFx^0,0x^0,0x^0,0=⎣⎢⎢⎢⎢⎢⎢⎡000000⎦⎥⎥⎥⎥⎥⎥⎤
P 1 , 0 = F P 0 , 0 F T + Q = [ 1125 750 250 0 0 0 750 1000 500 0 0 0 250 500 500 0 0 0 0 0 0 1125 750 250 0 0 0 750 1000 500 0 0 0 250 500 500 ] \pmb{P_{1,0}} = \pmb{F} \pmb{P_{0,0}} \pmb{F}^T + \pmb{Q} = \begin{bmatrix} 1125 & 750 & 250 & 0 & 0 & 0 \\ 750 & 1000 & 500 & 0 & 0 & 0 \\ 250 & 500 & 500 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1125 & 750 & 250 \\ 0 & 0 & 0 & 750 & 1000 & 500 \\ 0 & 0 & 0 & 250 & 500 & 500 \end{bmatrix} P1,0P1,0P1,0=FFFP0,0P0,0P0,0FFFT+QQQ=⎣⎢⎢⎢⎢⎢⎢⎡1125750250000750100050000025050050000000011257502500007501000500000250500500⎦⎥⎥⎥⎥⎥⎥⎤
第 1 次迭代
步骤1 - 测量
测量值: z 1 = [ − 375.93 301.78 ] \pmb{z_1} = \begin{bmatrix} -375.93 \\ 301.78 \end{bmatrix} z1z1z1=[−375.93301.78]
步骤2 - 更新
卡尔曼增益的计算: K 1 = P 1 , 0 H T ( H P 1 , 0 H T + R ) − 1 = [ 0.9921 0 0.6614 0 0.2205 0 0 0.9921 0 0.6614 0 0.2205 ] \pmb{K_1} = \pmb{P_{1,0}} \pmb{H}^T (\pmb{H} \pmb{P_{1,0}} \pmb{H}^T + \pmb{R})^{-1} = \begin{bmatrix} 0.9921 & 0 \\ 0.6614 & 0 \\ 0.2205 & 0 \\ 0 & 0.9921 \\ 0 & 0.6614 \\ 0 & 0.2205 \end{bmatrix} K1K1K1=P1,0P1,0P1,0HHHT(HHHP1,0P1,0P1,0HHHT+RRR)−1=⎣⎢⎢⎢⎢⎢⎢⎡0.99210.66140.22050000000.99210.66140.2205⎦⎥⎥⎥⎥⎥⎥⎤
对于位置的卡尔曼增益是 0.9921 0.9921 0.9921,意味着这一次测量的权重远大于估计的权重,估计的权重几乎可以忽略。
估计当前状态: x ^ 1 , 1 = x ^ 1 , 0 + K 1 ( z 1 − H x ^ 1 , 0 ) = [ − 390.54 − 260.36 − 86.8 298.02 198.7 66.23 ] \pmb{\hat{x}_{1,1}} = \pmb{\hat{x}_{1,0}} + \pmb{K_1}(\pmb{z_1} - \pmb{H} \pmb{\hat{x}_{1,0}}) = \begin{bmatrix} -390.54 \\ -260.36 \\ -86.8 \\ 298.02 \\ 198.7 \\ 66.23 \end{bmatrix} x^1,1x^1,1x^1,1=x^1,0x^1,0x^1,0+K1K1K1(z1z1z1−HHHx^1,0x^1,0x^1,0)=⎣⎢⎢⎢⎢⎢⎢⎡−390.54−260.36−86.8298.02198.766.23⎦⎥⎥⎥⎥⎥⎥⎤
更新当前估计不确定性: P 1 , 1 = ( I − K 1 H ) P 1 , 0 ( I − K 1 H ) T + K 1 R K 1 T = [ 8.93 750 2 0 0 0 750 504 334.7 0 0 0 2 334.7 444.9 0 0 0 0 0 0 8.93 750 2 0 0 0 750 504 334.7 0 0 0 2 334.7 444.9 ] \pmb{P_{1,1}} = (\pmb{I}-\pmb{K_1H}) \pmb{P_{1,0}} (\pmb{I} - \pmb{K_1H})^T + \pmb{K_1}\pmb{R}\pmb{K_1}^T = \begin{bmatrix} 8.93 & 750 & 2 & 0 & 0 & 0 \\ 750 & 504 & 334.7 & 0 & 0 & 0 \\ 2 & 334.7& 444.9 & 0 & 0 & 0 \\ 0 & 0 & 0 & 8.93 & 750 & 2 \\ 0 & 0 & 0 & 750 & 504 & 334.7 \\ 0 & 0 & 0 & 2 & 334.7 & 444.9 \end{bmatrix} P1,1P1,1P1,1=(III−K1HK1HK1H)P1,0P1,0P1,0(III−K1HK1HK1H)T+K1K1K1RRRK1K1K1T=⎣⎢⎢⎢⎢⎢⎢⎡8.937502000750504334.70002334.7444.90000008.937502000750504334.70002334.7444.9⎦⎥⎥⎥⎥⎥⎥⎤
步骤3 - 预测
x ^ 2 , 1 = F x ^ 1 , 1 = [ − 694.3 − 347.15 − 86.8 529.8 264.9 66.23 ] \pmb{\hat{x}_{2,1}} = \pmb{F} \pmb{\hat{x}_{1,1}} = \begin{bmatrix} -694.3 \\ -347.15 \\ -86.8 \\ 529.8 \\ 264.9 \\ 66.23 \end{bmatrix} x^2,1x^2,1x^2,1=FFFx^1,1x^1,1x^1,1=⎣⎢⎢⎢⎢⎢⎢⎡−694.3−347.15−86.8529.8264.966.23⎦⎥⎥⎥⎥⎥⎥⎤
P 2 , 1 = F P 1 , 1 F T + Q = [ 972 1236 559 0 0 0 1236 1618 780 0 0 0 559 780 445 0 0 0 0 0 0 972 1236 559 0 0 0 1236 1618 780 0 0 0 559 780 445 ] \pmb{P_{2,1}} = \pmb{F} \pmb{P_{1,1}} \pmb{F}^T + \pmb{Q} = \begin{bmatrix} 972 & 1236 & 559 & 0 & 0 & 0 \\ 1236 & 1618 & 780 & 0 & 0 & 0 \\ 559 & 780 & 445 & 0 & 0 & 0 \\ 0 & 0 & 0 & 972 & 1236 & 559\\ 0 & 0 & 0 & 1236 & 1618 & 780\\ 0 & 0 & 0 & 559 & 780 & 445 \end{bmatrix} P2,1P2,1P2,1=FFFP1,1P1,1P1,1FFFT+QQQ=⎣⎢⎢⎢⎢⎢⎢⎡972123655900012361618780000559780445000000972123655900012361618780000559780445⎦⎥⎥⎥⎥⎥⎥⎤
预测不确定性仍然非常高。
第 2 次迭代
步骤1 - 测量
测量值: z 2 = [ − 393.66 300.4 ] \pmb{z_2} = \begin{bmatrix} -393.66 \\ 300.4 \end{bmatrix} z2z2z2=[−393.66300.4]
步骤2 - 更新
卡尔曼增益的计算: K 2 = P 2 , 1 H T ( H P 2 , 1 H T + R ) − 1 = [ 0.9908 0 1.26 0 0.57 0 0 0.9908 0 1.26 0 0.57 ] \pmb{K_2} = \pmb{P_{2,1}} \pmb{H}^T (\pmb{H} \pmb{P_{2,1}} \pmb{H}^T + \pmb{R})^{-1} = \begin{bmatrix} 0.9908 & 0 \\ 1.26 & 0 \\ 0.57 & 0 \\ 0 & 0.9908 \\ 0 & 1.26 \\ 0 & 0.57 \end{bmatrix} K2K2K2=P2,1P2,1P2,1HHHT(HHHP2,1P2,1P2,1HHHT+RRR)−1=⎣⎢⎢⎢⎢⎢⎢⎡0.99081.260.570000000.99081.260.57⎦⎥⎥⎥⎥⎥⎥⎤
估计当前状态: x ^ 2 , 2 = x ^ 2 , 1 + K 2 ( z 2 − H x ^ 2 , 1 ) = [ − 378.9 53.8 94.5 303.9 − 22.3 − 63.6 ] \pmb{\hat{x}_{2,2}} = \pmb{\hat{x}_{2,1}} + \pmb{K_2}(\pmb{z_2} - \pmb{H} \pmb{\hat{x}_{2,1}}) = \begin{bmatrix} -378.9 \\ 53.8 \\ 94.5 \\ 303.9 \\ -22.3 \\ -63.6 \end{bmatrix} x^2,2x^2,2x^2,2=x^2,1x^2,1x^2,1+K2K2K2(z2z2z2−HHHx^2,1x^2,1x^2,1)=⎣⎢⎢⎢⎢⎢⎢⎡−378.953.894.5303.9−22.3−63.6⎦⎥⎥⎥⎥⎥⎥⎤
更新当前估计不确定性: P 2 , 2 = ( I − K 2 H ) P 2 , 1 ( I − K 2 H ) T + K 2 R K 2 T = [ 8.92 11.33 5.13 0 0 0 11.33 61.1 75.4 0 0 0 5.13 75.4 126.5 0 0 0 0 0 0 8.92 11.33 5.13 0 0 0 11.33 61.1 75.4 0 0 0 5.13 75.4 126.5 ] \pmb{P_{2,2}} = (\pmb{I}-\pmb{K_2H}) \pmb{P_{2,1}} (\pmb{I} - \pmb{K_2H})^T + \pmb{K_2}\pmb{R}\pmb{K_2}^T = \begin{bmatrix} 8.92 & 11.33 & 5.13 & 0 & 0 & 0 \\ 11.33 & 61.1 & 75.4 & 0 & 0 & 0 \\ 5.13 & 75.4 & 126.5 & 0 & 0 & 0 \\ 0 & 0 & 0 & 8.92 & 11.33 & 5.13 \\ 0 & 0 & 0 & 11.33 & 61.1 & 75.4 \\ 0 & 0 & 0 & 5.13 & 75.4 & 126.5 \end{bmatrix} P2,2P2,2P2,2=(III−K2HK2HK2H)P2,1P2,1P2,1(III−K2HK2HK2H)T+K2K2K2RRRK2K2K2T=⎣⎢⎢⎢⎢⎢⎢⎡8.9211.335.1300011.3361.175.40005.1375.4126.50000008.9211.335.1300011.3361.175.40005.1375.4126.5⎦⎥⎥⎥⎥⎥⎥⎤
步骤3 - 预测
x ^ 3 , 2 = F x ^ 2 , 2 = [ − 277.8 148.3 94.5 249.8 − 85.9 − 63.6 ] \pmb{\hat{x}_{3,2}} = \pmb{F} \pmb{\hat{x}_{2,2}} = \begin{bmatrix} -277.8 \\ 148.3 \\ 94.5 \\ 249.8 \\ -85.9 \\ -63.6 \end{bmatrix} x^3,2x^3,2x^3,2=FFFx^2,2x^2,2x^2,2=⎣⎢⎢⎢⎢⎢⎢⎡−277.8148.394.5249.8−85.9−63.6⎦⎥⎥⎥⎥⎥⎥⎤
P 3 , 2 = F P 2 , 2 F T + Q = [ 204.9 254 143.8 0 0 0 254 338.5 202 0 0 0 143.8 202 126.5 0 0 0 0 0 0 204.9 254 143.8 0 0 0 254 338.5 202 0 0 0 143.8 202 126.5 ] \pmb{P_{3,2}} = \pmb{F} \pmb{P_{2,2}} \pmb{F}^T + \pmb{Q} = \begin{bmatrix} 204.9 & 254 & 143.8 & 0 & 0 & 0 \\ 254 & 338.5 & 202 & 0 & 0 & 0 \\ 143.8 & 202 & 126.5 & 0 & 0 & 0 \\ 0 & 0 & 0 & 204.9 & 254 & 143.8 \\ 0 & 0 & 0 & 254 & 338.5 & 202 \\ 0 & 0 & 0 & 143.8 & 202 & 126.5 \end{bmatrix} P3,2P3,2P3,2=FFFP2,2P2,2P2,2FFFT+QQQ=⎣⎢⎢⎢⎢⎢⎢⎡204.9254143.8000254338.5202000143.8202126.5000000204.9254143.8000254338.5202000143.8202126.5⎦⎥⎥⎥⎥⎥⎥⎤
此时预测不确定性依然很高,是时候跳到最后一轮迭代看看了。
第 35 次迭代
步骤1 - 测量
测量值: z 35 = [ 299.89 2.14 ] \pmb{z_{35}} = \begin{bmatrix} 299.89 \\ 2.14 \end{bmatrix} z35z35z35=[299.892.14]
步骤2 - 更新
卡尔曼增益的计算: K 35 = P 35 , 34 H T ( H P 35 , 34 H T + R ) − 1 = [ 0.5556 0 0.2222 0 0.0444 0 0 0.5556 0 0.2222 0 0.0444 ] \pmb{K_{35}} = \pmb{P_{35,34}} \pmb{H}^T (\pmb{H} \pmb{P_{35,34}} \pmb{H}^T + \pmb{R})^{-1} = \begin{bmatrix} 0.5556& 0 \\ 0.2222& 0 \\ 0.0444 & 0 \\ 0 & 0.5556 \\ 0 & 0.2222 \\ 0 & 0.0444 \end{bmatrix} K35K35K35=P35,34P35,34P35,34HHHT(HHHP35,34P35,34P35,34HHHT+RRR)−1=⎣⎢⎢⎢⎢⎢⎢⎡0.55560.22220.04440000000.55560.22220.0444⎦⎥⎥⎥⎥⎥⎥⎤
对于位置的卡尔曼增益已经收敛于 0.56 0.56 0.56,这意味着测量权重和估计权重几乎相同。
估计当前状态: x ^ 35 , 35 = x ^ 35 , 34 + K 35 ( z 35 − H x ^ 35 , 34 ) = [ 299.2 0.25 − 1.9 3.3 − 25.5 − 0.64 ] \pmb{\hat{x}_{35,35}} = \pmb{\hat{x}_{35,34}} + \pmb{K_{35}}(\pmb{z_{35}} - \pmb{H} \pmb{\hat{x}_{35,34}}) = \begin{bmatrix} 299.2 \\ 0.25 \\ -1.9 \\ 3.3 \\ -25.5 \\ -0.64 \end{bmatrix} x^35,35x^35,35x^35,35=x^35,34x^35,34x^35,34+K35K35K35(z35z35z35−HHHx^35,34x^35,34x^35,34)=⎣⎢⎢⎢⎢⎢⎢⎡299.20.25−1.93.3−25.5−0.64⎦⎥⎥⎥⎥⎥⎥⎤
更新当前估计不确定性: P 35 , 35 = ( I − K 35 H ) P 35 , 34 ( I − K 35 H ) T + K 35 R K 35 T = [ 5 2 0.4 0 0 0 2 1.4 0.4 0 0 0 0.4 0.4 0.16 0 0 0 0 0 0 5 2 0.4 0 0 0 2 1.4 0.4 0 0 0 0.4 0.4 0.16 ] \pmb{P_{35,35}} = (\pmb{I}-\pmb{K_{35}H}) \pmb{P_{35,34}} (\pmb{I} - \pmb{K_{35}H})^T + \pmb{K_{35}}\pmb{R}\pmb{K_{35}}^T = \begin{bmatrix} 5 & 2 & 0.4 & 0 & 0 & 0 \\ 2 & 1.4 & 0.4 & 0 & 0 & 0 \\ 0.4 & 0.4 & 0.16 & 0 & 0 & 0 \\ 0 & 0 & 0 & 5 & 2 & 0.4 \\ 0 & 0 & 0 & 2 & 1.4 & 0.4 \\ 0 & 0 & 0 & 0.4 & 0.4 & 0.16 \end{bmatrix} P35,35P35,35P35,35=(III−K35HK35HK35H)P35,34P35,34P35,34(III−K35HK35HK35H)T+K35K35K35RRRK35K35K35T=⎣⎢⎢⎢⎢⎢⎢⎡520.400021.40.40000.40.40.16000000520.400021.40.40000.40.40.16⎦⎥⎥⎥⎥⎥⎥⎤
此刻位置不确定性 p x = p y = 5 p_x = p_y = 5 px=py=5,意味着预测的标准差为 5 m \sqrt{5} m 5m 。
步骤3 - 预测
x ^ 36 , 35 = F x ^ 35 , 35 = [ 298.5 − 1.65 − 1.9 − 22.5 − 26.1 − 0.64 ] \pmb{\hat{x}_{36,35}} = \pmb{F} \pmb{\hat{x}_{35,35}} = \begin{bmatrix} 298.5 \\ -1.65 \\ -1.9 \\ -22.5 \\ -26.1 \\ -0.64 \end{bmatrix} x^36,35x^36,35x^36,35=FFFx^35,35x^35,35x^35,35=⎣⎢⎢⎢⎢⎢⎢⎡298.5−1.65−1.9−22.5−26.1−0.64⎦⎥⎥⎥⎥⎥⎥⎤
P 36 , 35 = F P 35 , 35 F T + Q = [ 11.25 4.5 0.9 0 0 0 4.5 2.4 0.6 0 0 0 0.9 0.6 0.2 0 0 0 0 0 0 11.25 4.5 0.9 0 0 0 4.5 2.4 0.6 0 0 0 0.9 0.6 0.2 ] \pmb{P_{36,35}} = \pmb{F} \pmb{P_{35,35}} \pmb{F}^T + \pmb{Q} = \begin{bmatrix} 11.25 & 4.5 & 0.9 & 0 & 0 & 0 \\ 4.5 & 2.4 & 0.6 & 0 & 0 & 0 \\ 0.9 & 0.6 & 0.2 & 0 & 0 & 0 \\ 0 & 0 & 0 & 11.25 & 4.5 & 0.9 \\ 0 & 0 & 0 & 4.5 & 2.4 & 0.6 \\ 0 & 0 & 0 & 0.9 & 0.6 & 0.2 \end{bmatrix} P36,35P36,35P36,35=FFFP35,35P35,35P35,35FFFT+QQQ=⎣⎢⎢⎢⎢⎢⎢⎡11.254.50.90004.52.40.60000.90.60.200000011.254.50.90004.52.40.60000.90.60.2⎦⎥⎥⎥⎥⎥⎥⎤
本例总结
下表比较了车辆位置的真实值,测量值,估计值。
可以看到,卡尔曼滤波器很好地跟踪了车子,但是,当车子开始转弯操作时,估算值并不是那么准确。一段时间后,卡尔曼滤波器的精度提高了。
车辆沿直线行驶时,加速度恒定且等于零。但是,在转弯操作期间,车辆由于圆周运动而受到加速度:角加速度。
回想一下物理知识,角加速度为: α = Δ V R Δ t \alpha = \frac{\Delta V}{R \Delta t} α=RΔtΔV,其中: Δ t \Delta t Δt 是时间间隔, Δ V \Delta V ΔV 是一角间隔内的速度差, R R R 是圆半径。
尽管角加速度是恒定的,但是 x x x 和 y y y 轴上的角加速度投影不是恒定的,因此 x ¨ \ddot{x} x¨ 和 y ¨ \ddot{y} y¨ 并不是恒定的。
我们这次的卡尔曼滤波器专为恒定加速度模型而设计,尽管这里出现了角加速度,由于正确选择了 σ a 2 \sigma^2_a σa2 参数,它仍能成功跟踪机动的车子。
读者可以试试用程序实现这个例子,看看不同的 R R R 的 σ a 2 \sigma^2_a σa2 如何影响实际的卡尔曼滤波器的精度、卡尔曼增益收敛、估计不确定性。
例10 - 车子位置估计
这个例子里我们将估计火箭的高度,火箭上面有一个仪器,可以提供火箭高度的测量值。另外,还有一个加速度计,可以测量火箭的加速度。
加速计用作卡尔曼滤波器的控制输入(control input).
假设模型是恒定加速度的。
加速度计不能感应重力,所以我们需要每次从加速度计的测量值中,减去重力加速度 g g g 。
例如,把加速度计静止放在桌子上时,它会测到 1 1 1 个 g g g 的上升力,自由落体的加速度计测到的结果是 0 0 0。
在时间点 1 1 1 时,加速度计的测量值为: a n = x ¨ − g + ϵ a_n = \ddot{x} - g + \epsilon an=x¨−g+ϵ其中:
∙
\bullet
∙
x
¨
\ddot{x}
x¨ 是目标的实际加速度(目标的位置的二阶导)
∙
\bullet
∙
g
g
g 是重力加速度常量;
g
=
−
9.8
m
s
2
g = -9.8 \dfrac{m}{s^2}
g=−9.8s2m
∙
\bullet
∙
ϵ
\epsilon
ϵ 是加速度计的测量误差
状态外推方程
矩阵表示法中状态外推方程的一般形式为:
x
^
n
+
1
,
n
=
F
x
^
n
,
n
+
G
u
n
+
w
n
\pmb{\hat{x}_{n+1,n}} = \pmb{F}\pmb{\hat{x}_{n,n}} + \pmb{Gu_n} + \pmb{w_n}
x^n+1,nx^n+1,nx^n+1,n=FFFx^n,nx^n,nx^n,n+GunGunGun+wnwnwn其中:
x
^
n
+
1
,
n
\pmb{\hat{x}_{n+1,n}}
x^n+1,nx^n+1,nx^n+1,n 是在时间点
n
+
1
n+1
n+1 的预测状态向量
x
^
n
,
n
\pmb{\hat{x}_{n,n}}
x^n,nx^n,nx^n,n 是在时间点
n
n
n 的估计状态向量
u
n
\pmb{u_n}
ununun 是控制变量
w
n
\pmb{w_n}
wnwnwn 是过程噪声
F
\pmb{F}
FFF 是状态过渡矩阵
G
\pmb{G}
GGG 是控制矩阵
在本例中,我们有控制变量 u \pmb{u} uuu,它基于加速度计的测量值。
系统的状态
x
n
\pmb{x_n}
xnxnxn 定义为:
x
n
=
[
x
n
x
˙
n
]
\pmb{x_n} = \begin{bmatrix}\ x_n \\ \dot{x}_n\end{bmatrix}
xnxnxn=[ xnx˙n] 其中:
x
n
x_n
xn 是在时间点
n
n
n 时火箭的高度
x
˙
n
\dot{x}_n
x˙n 是在时间点
n
n
n 时火箭的速度
我们可以将状态外推方程表示为: [ x ^ n + 1 , n x ˙ ^ n + 1 , n ] = [ 1 Δ t 0 1 ] [ x ^ n , n x ˙ ^ n , n ] + [ 0.5 Δ t 2 Δ t ] ( a n + g ) \begin{bmatrix} \hat{x}_{n+1,n} \\ \hat{\dot{x}}_{n+1,n} \end{bmatrix} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1\end{bmatrix} \begin{bmatrix} \hat{x}_{n,n} \\ \hat{\dot{x}}_{n,n} \end{bmatrix} + \begin{bmatrix} 0.5\Delta t^2 \\ \Delta t \end{bmatrix} (a_n +g) [x^n+1,nx˙^n+1,n]=[10Δt1][x^n,nx˙^n,n]+[0.5Δt2Δt](an+g)
上述方程中: F = [ 1 Δ t 0 1 ] \pmb{F} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} FFF=[10Δt1] G = [ 0.5 Δ t 2 Δ t ] \pmb{G} = \begin{bmatrix} 0.5\Delta t^2 \\ \Delta t \end{bmatrix} GGG=[0.5Δt2Δt] u n = ( a n + g ) \pmb{u_n} = (a_n + g) ununun=(an+g)
协方差外推方程
协方差外推方程的一般形式为:
P
n
+
1
,
n
=
F
P
n
,
n
F
T
+
Q
\pmb{P_{n+1,n} = FP_{n,n}F^T + Q}
Pn+1,n=FPn,nFT+QPn+1,n=FPn,nFT+QPn+1,n=FPn,nFT+Q其中:
P
n
,
n
\pmb{P_{n,n}}
Pn,nPn,nPn,n 是当前状态的估计不确定性(协方差)矩阵
P
n
+
1
,
n
\pmb{P_{n+1,n}}
Pn+1,nPn+1,nPn+1,n 是在当前预测的下一个状态的估计不确定性(协方差)矩阵
F
\pmb{F}
FFF 是之前在“线性动态系统模型”部分推导出来的状态转移矩阵
Q
\pmb{Q}
QQQ 是过程噪声矩阵
估计不确定性的矩阵形式为:
P
=
[
p
x
p
x
x
˙
p
x
˙
x
p
x
˙
]
\pmb{P} = \begin{bmatrix} p_x & p_{x\dot{x}} \\ p_{\dot{x}x} & p_{\dot{x}} \end{bmatrix}
PPP=[pxpx˙xpxx˙px˙]
矩阵对角线上的元素是估计值的方差:
∙
\bullet
∙
p
x
p_x
px 是高度估计的方差
∙
\bullet
∙
p
x
˙
p_{\dot{x}}
px˙ 是速度估计的方差
∙
\bullet
∙ 非对角线上的元素是协方差
过程噪声矩阵
这里假设它是离散噪声模型:不同时间段的噪声是不同的,在一个时间段里面是恒定的。
恒定加速度的过程噪声矩阵如下: Q = [ σ x 2 σ x x ˙ 2 σ x ˙ x 2 σ x 2 ] \pmb{Q} = \begin{bmatrix} \sigma^2_x & \sigma^2_{x\dot{x}} \\[0.5em] \sigma^2_{\dot{x}x} & \sigma^2_x \end{bmatrix} QQQ=[σx2σx˙x2σxx˙2σx2]
前面已经推导了恒定加速度模型的Q矩阵,本例的Q矩阵为:
Q
=
[
Δ
t
4
4
Δ
t
3
2
Δ
t
3
2
Δ
t
2
]
ϵ
2
\pmb{Q} = \begin{bmatrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} \\[0.5em] \frac{\Delta t^3}{2} & \Delta t^2 \end{bmatrix} \epsilon^2
QQQ=[4Δt42Δt32Δt3Δt2]ϵ2其中:
∙
\bullet
∙
Δ
t
\Delta t
Δt 是连续测量之间的时间间隔
∙
\bullet
∙
ϵ
2
\epsilon^2
ϵ2 是加速度计测量值的随机方差
在前面的例子里,我们使用了在加速度中的系统随机方差 σ a 2 \sigma^2_a σa2 作为过程噪声矩阵的乘数。但是在这里我们有加速度计,可以测量系统随机加速度。加速度计的误差v比系统随机加速度低得多,因此这里我们用 ϵ 2 \epsilon^2 ϵ2 作为过程噪声的乘数。
它使得我们的估计不确定性更低!
现在可以写出我们例子的协方差外推矩阵: P n + 1 , n = F P n , n F T + Q \pmb{P_{n+1,n} = FP_{n,n}F^T + Q} Pn+1,n=FPn,nFT+QPn+1,n=FPn,nFT+QPn+1,n=FPn,nFT+Q [ p x n + 1 , n p x x ˙ n + 1 , n p x ˙ x n + 1 , n p x ˙ n + 1 , n ] = [ 1 Δ t 0 1 ] × [ p x n , n p x x ˙ n , n p x ˙ x n , n p x ˙ n , n ] × [ 1 0 Δ t 1 ] + [ Δ t 4 4 Δ t 3 2 Δ t 3 2 Δ t 2 ] ϵ 2 \begin{bmatrix} p_{x_{n+1,n}} & p_{x\dot{x}_{n+1,n}} \\ p_{\dot{x}x_{n+1,n}} & p_{\dot{x}_{n+1,n}} \end{bmatrix} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1\end{bmatrix} \times \begin{bmatrix} p_{x_{n,n}} & p_{x\dot{x}_{n,n}} \\ p_{\dot{x}x_{n,n}} & p_{\dot{x}_{n,n}} \end{bmatrix} \times \begin{bmatrix} 1 & 0 \\ \Delta t & 1\end{bmatrix} + \begin{bmatrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} \\[0.5em] \frac{\Delta t^3}{2} & \Delta t^2 \end{bmatrix} \epsilon^2 [pxn+1,npx˙xn+1,npxx˙n+1,npx˙n+1,n]=[10Δt1]×[pxn,npx˙xn,npxx˙n,npx˙n,n]×[1Δt01]+[4Δt42Δt32Δt3Δt2]ϵ2
测量方程
矩阵形式的测量方程的一般形式为:
z
n
=
H
x
n
+
v
n
\pmb{z_n} = \pmb{H}\pmb{x_n} + \pmb{v_n}
znznzn=HHHxnxnxn+vnvnvn其中:
z
n
\pmb{z_n}
znznzn 是测量向量(measurement vector)
x
n
\pmb{x_n}
xnxnxn 是真实系统状态(隐藏状态)
v
n
\pmb{v_n}
vnvnvn 是随机噪声向量
H
\pmb{H}
HHH 是观察矩阵(observation matrix)
我们得到的测量值只有火箭的高度: z n = [ x n , m e a s u r e d ] \pmb{z_n} = \begin{bmatrix}x_{n,measured}\end{bmatrix} znznzn=[xn,measured] z n = H x n \pmb{z_n} = \pmb{H} \pmb{x_n} znznzn=HHHxnxnxn [ x n , m e a s u r e d ] = H [ x n x ˙ n ] [x_{n,measured}] = \pmb{H} \begin{bmatrix} x_n \\ \dot{x}_n \end{bmatrix} [xn,measured]=HHH[xnx˙n]
z n \pmb{z_n} znznzn 的维度是 1 × 1 1\times 1 1×1, x n \pmb{x_n} xnxnxn 的维度是 2 × 1 2\times 1 2×1, 因此观测矩阵 H \pmb{H} HHH 的维度应该为 1 × 2 1\times 2 1×2 H = [ 1 0 ] \pmb{H} = \begin{bmatrix} 1 & 0\end{bmatrix} HHH=[10]
测量不确定性
测量协方差矩阵为: R n = [ σ x m 2 ] \pmb{R_n} = \begin{bmatrix} \sigma^2_{x_m} \end{bmatrix} RnRnRn=[σxm2] 下标 m m m 用于测量不确定性。
为了简化例子,假设测量不确定性是恒定的: R 1 = R 2 … R n − 1 = R n = R \pmb{R_1} = \pmb{R_2}\dots\pmb{R_{n-1}} = \pmb{R_n} = \pmb{R} R1R1R1=R2R2R2…Rn−1Rn−1Rn−1=RnRnRn=RRR
卡尔曼增益
矩阵形式的卡尔曼增益如下:
K
n
=
P
n
,
n
−
1
H
T
(
H
P
n
,
n
−
1
H
T
+
R
n
)
−
1
\pmb{K_n} = \pmb{P_{n,n-1}} \pmb{H}^T (\pmb{HP_{n,n-1}H}^T + \pmb{R_n})^{-1}
KnKnKn=Pn,n−1Pn,n−1Pn,n−1HHHT(HPn,n−1HHPn,n−1HHPn,n−1HT+RnRnRn)−1其中:
K
n
\pmb{K_n}
KnKnKn 是卡尔曼增益
P
n
,
n
−
1
\pmb{P_{n,n-1}}
Pn,n−1Pn,n−1Pn,n−1 是在当前时刻的先前估计不确定性(方差)矩阵(在上一个状态预测得到的)
H
\pmb{H}
HHH 是观察矩阵
R
n
\pmb{R_n}
RnRnRn 是测量不确定性(测量噪声协方差矩阵)
前面已经导出了卡尔曼增益所需要的元素: [ K 1 , 1 n K 2 , 1 n ] = [ p x n , n − 1 p x x ˙ n , n − 1 p x ˙ x n , n − 1 p x ˙ n , n − 1 ] [ 1 0 ] × ( [ 1 0 ] [ p x n , n − 1 p x x ˙ n , n − 1 p x ˙ x n , n − 1 p x ˙ n , n − 1 ] [ 1 0 ] + [ σ x m 2 ] ) − 1 \begin{bmatrix} K_{1,1_n} \\ K_{2,1_n}\end{bmatrix} = \begin{bmatrix} p_{x_{n,n-1}} & p_{x\dot{x}_{n,n-1}} \\ p_{\dot{x}x_{n,n-1}} & p_{\dot{x}_{n,n-1}} \end{bmatrix} \begin{bmatrix} 1 \\ 0 \end{bmatrix} \times \left( \begin{bmatrix} 1 & 0 \end{bmatrix} \begin{bmatrix} p_{x_{n,n-1}} & p_{x\dot{x}_{n,n-1}} \\ p_{\dot{x}x_{n,n-1}} & p_{\dot{x}_{n,n-1}} \end{bmatrix} \begin{bmatrix} 1 \\ 0 \end{bmatrix} + [\sigma^2_{x_m}] \right)^{-1} [K1,1nK2,1n]=[pxn,n−1px˙xn,n−1pxx˙n,n−1px˙n,n−1][10]×([10][pxn,n−1px˙xn,n−1pxx˙n,n−1px˙n,n−1][10]+[σxm2])−1
状态更新方程
状态更新方程的矩阵形式为:
x
^
n
,
n
=
x
^
n
,
n
−
1
+
K
n
(
z
n
−
H
x
^
n
,
n
−
1
)
\pmb{\hat{x}_{n,n}} = \pmb{\hat{x}_{n,n-1}} + \pmb{K_n}(\pmb{z_n} - \pmb{H} \pmb{\hat{x}_{n,n-1}})
x^n,nx^n,nx^n,n=x^n,n−1x^n,n−1x^n,n−1+KnKnKn(znznzn−HHHx^n,n−1x^n,n−1x^n,n−1)其中:
x
^
n
,
n
\pmb{\hat{x}_{n,n}}
x^n,nx^n,nx^n,n 是在时刻
n
n
n 的估计系统状态向量
x
^
n
,
n
−
1
\pmb{\hat{x}_{n,n-1}}
x^n,n−1x^n,n−1x^n,n−1 是在时刻
n
−
1
n-1
n−1 的预测系统状态向量
K
n
\pmb{K_n}
KnKnKn 是卡尔曼增益
z
n
\pmb{z_n}
znznzn 是测量值
H
\pmb{H}
HHH 是观察矩阵(observation matrix)
构成状态更新方程的元素已经在前面全部定义好了。
协方差更新方程
P
n
,
n
=
(
I
−
K
n
H
)
P
n
,
n
−
1
(
I
−
K
n
H
)
T
+
K
n
R
n
K
n
T
\pmb{P_{n,n}} = (\pmb{I}-\pmb{K_nH}) \pmb{P_{n,n-1}} (\pmb{I} - \pmb{K_nH})^T + \pmb{K_n}\pmb{R_n}\pmb{K_n}^T
Pn,nPn,nPn,n=(III−KnHKnHKnH)Pn,n−1Pn,n−1Pn,n−1(III−KnHKnHKnH)T+KnKnKnRnRnRnKnKnKnT 其中
P
n
,
n
\pmb{P_{n,n}}
Pn,nPn,nPn,n 是当前状态的估计不确定性(协方差)矩阵
P
n
,
n
−
1
\pmb{P_{n,n-1}}
Pn,n−1Pn,n−1Pn,n−1 是在当前状态下,先前估计不确定性(协方差)矩阵 (在上一个状态时预测得到的)
K
n
\pmb{K_n}
KnKnKn 是卡尔曼增益
H
\pmb{H}
HHH 是观察矩阵
R
n
\pmb{R_n}
RnRnRn 是测量不确定性(测量噪声协方差矩阵)
构成协方差更新方程的所有元素已经在前面全部定义好了。
数值实例
假设火箭以恒定加速度垂直助推,火箭上安装了加速度计,可以提供高度的测量值和加速度的测量值,作为卡尔曼滤波器的控制输入。
∙
\bullet
∙ 测量时间间隔为
Δ
t
=
0.25
s
\Delta t = 0.25s
Δt=0.25s
∙
\bullet
∙ 火箭加速度
x
¨
=
30
m
s
2
\ddot{x} = 30 \dfrac{m}{s^2}
x¨=30s2m
∙
\bullet
∙ 高度的测量误差标准差:
σ
x
m
=
20
m
\sigma_{x_m} = 20m
σxm=20m
∙
\bullet
∙ 加速度计的测量误差标准差:
ϵ
=
0.1
m
s
2
\epsilon = 0.1\dfrac{m}{s^2}
ϵ=0.1s2m
∙
\bullet
∙ 状态过渡矩阵
F
\pmb{F}
FFF 为:
F
=
[
1
Δ
t
0
1
]
=
[
1
0.25
0
1
]
\pmb{F} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} 1 & 0.25 \\ 0 & 1 \end{bmatrix}
FFF=[10Δt1]=[100.251]
∙
\bullet
∙ 控制矩阵
B
\pmb{B}
BBB 为:
G
=
[
0.5
Δ
t
2
Δ
t
]
=
[
0.0313
0.25
]
\pmb{G} = \begin{bmatrix} 0.5\Delta t^2 \\ \Delta t \end{bmatrix} = \begin{bmatrix} 0.0313 \\ 0.25 \end{bmatrix}
GGG=[0.5Δt2Δt]=[0.03130.25]
∙
\bullet
∙ 过程噪声矩阵
Q
\pmb{Q}
QQQ 为:
Q
=
[
Δ
t
4
4
Δ
t
3
2
Δ
t
3
2
Δ
t
2
]
σ
a
2
=
[
0.2
5
4
4
0.2
5
3
2
0.2
5
3
2
0.2
5
2
]
0.
1
2
\pmb{Q} = \begin{bmatrix} \dfrac{\Delta t^4}{4} & \dfrac{\Delta t^3}{2} \\[0.7em] \dfrac{\Delta t^3}{2} & \Delta t^2 \end{bmatrix} \sigma^2_a = \begin{bmatrix} \dfrac{0.25^4}{4} & \dfrac{0.25^3}{2} \\[0.7em] \dfrac{0.25^3}{2} & 0.25^2 \end{bmatrix} 0.1^2
QQQ=⎣⎢⎢⎡4Δt42Δt32Δt3Δt2⎦⎥⎥⎤σa2=⎣⎢⎢⎡40.25420.25320.2530.252⎦⎥⎥⎤0.12
∙ \bullet ∙ 测量不确定性 R \pmb{R} RRR 为: R n = R = [ σ x m 2 ] = 400 \pmb{R_n} = \pmb{R} = [\sigma^2_{x_m}] = 400 RnRnRn=RRR=[σxm2]=400
下面的表格是 30 次含噪声的测量值:
1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | ||
---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|
高度 | x n ( m ) x_n(m) xn(m) | -32.4 | -11.1 | 18 | 22.9 | 19.5 | 28.6 | 46.5 | 68.9 | 48.2 | 56.1 | 90.5 | 104.9 | 140.9 | 148 | 187.6 | 209.2 | 244.6 | 276.4 | 323.5 | 357.3 | 357.4 | 398.3 | 446.7 | 465.1 | 529.4 | 570.4 | 636.8 | 693.3 | 707.3 | 748.5 |
加速度 | a n ( m s 2 ) a_n(\dfrac{m}{s^2}) an(s2m) | 39.72 | 40.02 | 39.97 | 39.81 | 39.75 | 39.6 | 39.77 | 39.83 | 39.73 | 39.87 | 39.81 | 39.92 | 39.78 | 39.98 | 39.76 | 39.86 | 39.61 | 39.86 | 39.74 | 39.87 | 39.63 | 39.67 | 39.96 | 39.8 | 39.89 | 39.85 | 39.9 | 39.81 | 39.81 | 39.68 |
第 0 次迭代
初始化
我们不知道火箭的位置;初始位置和速度都设为 0 0 0:
x ^ 0 , 0 = [ 0 0 ] \pmb{\hat{x}_{0,0}}=\begin{bmatrix} 0 \\ 0\end{bmatrix} x^0,0x^0,0x^0,0=[00]
火箭的加速度也不知道,但是我们可以推测它会比 0 0 0 大,所以假设:
u 0 = g \pmb{u_0} = g u0u0u0=g
由于初始状态向量是个猜测值,我们将会设置很高的估计不确定性。高的估计不确定性会导致高卡尔曼增益,给到测量值很高的权重。 P 0 , 0 = [ 500 0 0 500 ] \pmb{P_{0,0}} = \begin{bmatrix} 500 & 0 \\ 0 & 500 \end{bmatrix} P0,0P0,0P0,0=[50000500]
预测
现在我们可以基于初始值预测下一个状态: x ^ 1 , 0 = F x ^ 0 , 0 + G u 0 = [ 1 0.25 0 1 ] [ 0 0 ] + [ 0.0313 0.25 ] 9.8 = [ 0.3 2.45 ] \begin{aligned} \pmb{\hat{x}_{1,0}} &= \pmb{F} \pmb{\hat{x}_{0,0}} + \pmb{G} \pmb{u_0} \\ &= \begin{bmatrix} 1 & 0.25 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} 0 \\ 0 \end{bmatrix} + \begin{bmatrix} 0.0313 \\ 0.25 \end{bmatrix} 9.8 \\ &= \begin{bmatrix} 0.3 \\ 2.45 \end{bmatrix} \end{aligned} x^1,0x^1,0x^1,0=FFFx^0,0x^0,0x^0,0+GGGu0u0u0=[100.251][00]+[0.03130.25]9.8=[0.32.45]
P 1 , 0 = F P 0 , 0 F T + Q = [ 1 0.25 0 1 ] [ 500 0 0 500 ] [ 1 0 0.25 1 ] + [ 0.2 5 4 4 0.2 5 3 2 0.2 5 3 2 0.2 5 2 ] 0. 1 2 = [ 531.25 125 125 500 ] \begin{aligned} \pmb{P_{1,0}} &= \pmb{F} \pmb{P_{0,0}} \pmb{F}^T + \pmb{Q} \\ &= \begin{bmatrix} 1 & 0.25 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} 500 & 0 \\ 0 & 500 \end{bmatrix} \begin{bmatrix} 1 & 0 \\ 0.25 & 1 \end{bmatrix} + \begin{bmatrix} \dfrac{0.25^4}{4} & \dfrac{0.25^3}{2} \\[0.7em] \dfrac{0.25^3}{2} & 0.25^2 \end{bmatrix} 0.1^2 \\ &= \begin{bmatrix} 531.25 & 125 \\ 125 & 500 \end{bmatrix} \end{aligned} P1,0P1,0P1,0=FFFP0,0P0,0P0,0FFFT+QQQ=[100.251][50000500][10.2501]+⎣⎢⎢⎡40.25420.25320.2530.252⎦⎥⎥⎤0.12=[531.25125125500]
第 1 次迭代
步骤1 - 测量
测量值: z 1 = − 32.40 u 1 = 39.72 \pmb{z_1} = -32.40 \\ \pmb{u_1} = 39.72 z1z1z1=−32.40u1u1u1=39.72
步骤2 - 更新
卡尔曼增益的计算: K 1 = P 1 , 0 H T ( H P 1 , 0 H T + R ) − 1 = [ 0.57 0.13 ] \pmb{K_1} = \pmb{P_{1,0}} \pmb{H}^T (\pmb{H} \pmb{P_{1,0}} \pmb{H}^T + \pmb{R})^{-1} = \begin{bmatrix} 0.57 \\ 0.13 \\ \end{bmatrix} K1K1K1=P1,0P1,0P1,0HHHT(HHHP1,0P1,0P1,0HHHT+RRR)−1=[0.570.13]
估计当前状态: x ^ 1 , 1 = x ^ 1 , 0 + K 1 ( z 1 − H x ^ 1 , 0 ) = [ − 18.35 − 1.94 ] \pmb{\hat{x}_{1,1}} = \pmb{\hat{x}_{1,0}} + \pmb{K_1}(\pmb{z_1} - \pmb{H} \pmb{\hat{x}_{1,0}}) = \begin{bmatrix} -18.35 \\ -1.94 \end{bmatrix} x^1,1x^1,1x^1,1=x^1,0x^1,0x^1,0+K1K1K1(z1z1z1−HHHx^1,0x^1,0x^1,0)=[−18.35−1.94]
更新当前估计不确定性: P 1 , 1 = ( I − K 1 H ) P 1 , 0 ( I − K 1 H ) T + K 1 R K 1 T = [ 228.2 53.7 53.7 483.2 ] \pmb{P_{1,1}} = (\pmb{I}-\pmb{K_1H}) \pmb{P_{1,0}} (\pmb{I} - \pmb{K_1H})^T + \pmb{K_1}\pmb{R}\pmb{K_1}^T = \begin{bmatrix} 228.2 & 53.7 \\ 53.7 & 483.2 \end{bmatrix} P1,1P1,1P1,1=(III−K1HK1HK1H)P1,0P1,0P1,0(III−K1HK1HK1H)T+K1K1K1RRRK1K1K1T=[228.253.753.7483.2]
步骤3 - 预测
x ^ 2 , 1 = F x ^ 1 , 1 + G u 1 = [ 1 0.25 0 1 ] [ − 18.35 0.25 ] + [ 0.0313 0.25 ] ( 39.72 + ( − 9.8 ) ) = [ − 17.9 5.54 ] \begin{aligned} \pmb{\hat{x}_{2,1}} &= \pmb{F} \pmb{\hat{x}_{1,1}} + \pmb{G} \pmb{u_1} \\ &= \begin{bmatrix} 1 & 0.25 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} -18.35 \\ 0.25 \end{bmatrix} + \begin{bmatrix} 0.0313 \\ 0.25 \end{bmatrix} \left(39.72+(-9.8)\right) \\ &= \begin{bmatrix} -17.9 \\ 5.54 \end{bmatrix} \end{aligned} x^2,1x^2,1x^2,1=FFFx^1,1x^1,1x^1,1+GGGu1u1u1=[100.251][−18.350.25]+[0.03130.25](39.72+(−9.8))=[−17.95.54]
P 2 , 1 = F P 1 , 1 F T + Q = [ 285.2 174.5 174.5 483.2 ] \pmb{P_{2,1}} = \pmb{F} \pmb{P_{1,1}} \pmb{F}^T + \pmb{Q} = \begin{bmatrix} 285.2 & 174.5 \\ 174.5 & 483.2 \end{bmatrix} P2,1P2,1P2,1=FFFP1,1P1,1P1,1FFFT+QQQ=[285.2174.5174.5483.2]
预测不确定性仍然非常高。
第 2 次迭代
步骤1 - 测量
测量值: z 2 = − 11.1 u 2 = 40.02 \pmb{z_2} = -11.1 \\ \pmb{u_2} = 40.02 z2z2z2=−11.1u2u2u2=40.02
步骤2 - 更新
卡尔曼增益的计算: K 2 = P 2 , 1 H T ( H P 2 , 1 H T + R ) − 1 = [ 0.42 0.26 ] \pmb{K_2} = \pmb{P_{2,1}} \pmb{H}^T (\pmb{H} \pmb{P_{2,1}} \pmb{H}^T + \pmb{R})^{-1} = \begin{bmatrix} 0.42 \\ 0.26 \end{bmatrix} K2K2K2=P2,1P2,1P2,1HHHT(HHHP2,1P2,1P2,1HHHT+RRR)−1=[0.420.26]
估计当前状态: x ^ 2 , 2 = x ^ 2 , 1 + K 2 ( z 2 − H x ^ 2 , 1 ) = [ − 15.1 7.3 ] \pmb{\hat{x}_{2,2}} = \pmb{\hat{x}_{2,1}} + \pmb{K_2}(\pmb{z_2} - \pmb{H} \pmb{\hat{x}_{2,1}}) = \begin{bmatrix} -15.1 \\ 7.3 \end{bmatrix} x^2,2x^2,2x^2,2=x^2,1x^2,1x^2,1+K2K2K2(z2z2z2−HHHx^2,1x^2,1x^2,1)=[−15.17.3]
更新当前估计不确定性: P 2 , 2 = ( I − K 2 H ) P 2 , 1 ( I − K 2 H ) T + K 2 R K 2 T = [ 166.5 101.9 101.9 438.8 ] \pmb{P_{2,2}} = (\pmb{I}-\pmb{K_2H}) \pmb{P_{2,1}} (\pmb{I} - \pmb{K_2H})^T + \pmb{K_2}\pmb{R}\pmb{K_2}^T = \begin{bmatrix} 166.5 & 101.9 \\ 101.9 & 438.8 \end{bmatrix} P2,2P2,2P2,2=(III−K2HK2HK2H)P2,1P2,1P2,1(III−K2HK2HK2H)T+K2K2K2RRRK2K2K2T=[166.5101.9101.9438.8]
步骤3 - 预测
x ^ 3 , 2 = F x ^ 2 , 2 + G u 2 = [ − 12.3 14.8 ] \pmb{\hat{x}_{3,2}} = \pmb{F} \pmb{\hat{x}_{2,2}} + \pmb{G}\pmb{u_2} = \begin{bmatrix} -12.3 \\ 14.8 \end{bmatrix} x^3,2x^3,2x^3,2=FFFx^2,2x^2,2x^2,2+GGGu2u2u2=[−12.314.8]
P 3 , 2 = F P 2 , 2 F T + Q = [ 244.9 211.6 211.6 438.8 ] \pmb{P_{3,2}} = \pmb{F} \pmb{P_{2,2}} \pmb{F}^T + \pmb{Q} = \begin{bmatrix} 244.9 & 211.6 \\ 211.6 & 438.8 \end{bmatrix} P3,2P3,2P3,2=FFFP2,2P2,2P2,2FFFT+QQQ=[244.9211.6211.6438.8]
现在到最后一轮迭代看看。
第 30 次迭代
步骤1 - 测量
测量值: z 30 = 748.35 u 2 = 39.68 \pmb{z_{30}} = 748.35 \\ \pmb{u_2} = 39.68 z30z30z30=748.35u2u2u2=39.68
步骤2 - 更新
卡尔曼增益的计算: K 30 = P 30 , 29 H T ( H P 30 , 29 H T + R ) − 1 = [ 0.12 0.02 ] \pmb{K_{30}} = \pmb{P_{30,29}} \pmb{H}^T (\pmb{H} \pmb{P_{30,29}} \pmb{H}^T + \pmb{R})^{-1} = \begin{bmatrix} 0.12 \\ 0.02 \end{bmatrix} K30K30K30=P30,29P30,29P30,29HHHT(HHHP30,29P30,29P30,29HHHT+RRR)−1=[0.120.02]
高度值的卡尔曼增益汇聚到 0.12 0.12 0.12 ,意味着估计值的权重要比测量值的权重要高。
估计当前状态: x ^ 30 , 30 = x ^ 30 , 29 + K 30 ( z 30 − H x ^ 30 , 29 ) = [ 776.7 215.4 ] \pmb{\hat{x}_{30,30}} = \pmb{\hat{x}_{30,29}} + \pmb{K_{30}}(\pmb{z_{30}} - \pmb{H} \pmb{\hat{x}_{30,29}}) = \begin{bmatrix} 776.7 \\ 215.4 \end{bmatrix} x^30,30x^30,30x^30,30=x^30,29x^30,29x^30,29+K30K30K30(z30z30z30−HHHx^30,29x^30,29x^30,29)=[776.7215.4]
更新当前估计不确定性: P 30 , 30 = ( I − K 30 H ) P 30 , 29 ( I − K 30 H ) T + K 30 R K 30 T = [ 49.3 9.7 9.7 2.6 ] \pmb{P_{30,30}} = (\pmb{I}-\pmb{K_{30}H}) \pmb{P_{30,29}} (\pmb{I} - \pmb{K_{30}H})^T + \pmb{K_{30}}\pmb{R}\pmb{K_{30}}^T = \begin{bmatrix} 49.3 & 9.7 \\ 9.7 & 2.6 \end{bmatrix} P30,30P30,30P30,30=(III−K30HK30HK30H)P30,29P30,29P30,29(III−K30HK30HK30H)T+K30K30K30RRRK30K30K30T=[49.39.79.72.6]
此时高度的不确定性 p x = 49.3 p_x = 49.3 px=49.3,说明预测值的标准差为 49.3 m = 7.02 m \sqrt{49.3}m = 7.02m 49.3m=7.02m。(测量值的标准差是 20 m 20m 20m)
步骤3 - 预测
x ^ 31 , 30 = F x ^ 30 , 30 + G u 30 = [ 831.5 222.94 ] \pmb{\hat{x}_{31,30}} = \pmb{F} \pmb{\hat{x}_{30,30}} + \pmb{G}\pmb{u_{30}} = \begin{bmatrix} 831.5 \\ 222.94 \end{bmatrix} x^31,30x^31,30x^31,30=FFFx^30,30x^30,30x^30,30+GGGu30u30u30=[831.5222.94]
P 31 , 30 = F P 30 , 30 F T + Q = [ 54.3 10.4 10.4 2.6 ] \pmb{P_{31,30}} = \pmb{F} \pmb{P_{30,30}} \pmb{F}^T + \pmb{Q} = \begin{bmatrix} 54.3 & 10.4 \\ 10.4 & 2.6 \end{bmatrix} P31,30P31,30P31,30=FFFP30,30P30,30P30,30FFFT+QQQ=[54.310.410.42.6]
本例总结
下面的表格是火箭高度的真实值,测量值,估计值:
卡尔曼增益:
开始的时候,估计的高度会受到测量的影响,并且由于测量的噪声很大,因此与实际的火箭高度并不一致。
但随着Kalman增益的收敛,噪声测量的影响较小,估计的高度与真实高度吻合良好。
在这个例子中,我们没有任何操作导致加速度的改变,但是如果我们有,控制输入(加速计)将更新状态外推方程。