扩展卡尔曼滤波新手教程（二）
说明：本文内容翻译自外文网站The Extended Kalman Filter: An Interactive Tutorial for Non-Experts，仅供学习和参考。
本文是扩展卡尔曼滤波新手教程系列的第二篇，上一篇博客请看：扩展卡尔曼滤波新手教程（一）。
Part6: 预测和更新
我们现在已经快要准备好运行我们的卡尔曼滤波器了。但是，首先你可能想知道在最初的状态方程中，常数a去哪了：
x
k
=
a
x
k
−
1
x_k=ax_{k-1}
xk=axk−1
看上去它好像在用于状态估计的方程中销声匿迹了：
x
k
^
=
x
k
−
1
^
+
g
k
(
z
k
−
x
k
−
1
^
)
\hat{x_k}=\hat{x_{k-1}}+g_k(z_k-\hat{x_{k-1}})
xk^=xk−1^+gk(zk−xk−1^)
答案是，在估计状态时，这两个等式我们都需要。 事实上, 这两个等式表示了基于不同类型信息进行的状态估计。第一个等式表示的是针对系统状态应该是什么的预测，而第二个等式表示的是基于观测值对这个预测的更新（技术上来讲，第一个估计被称为先验（prior），第二个估计称为后验（posterior），而且大多数处理方法都引入了一些额外的上标或下标来显示这种区别。由于我试图使事情保持简单，并且易于用您最喜欢的编程语言编写代码，所以我尽量避免进一步使符号复杂化。）。所以我们在
x
x
x上方添加一个"
^
\hat {}
^"符号表明它是一个估计：
x
k
^
=
a
x
k
−
1
^
\hat{x_k}=a\hat{x_{k-1}}
xk^=axk−1^
最后，我们在误差预测中也使用了常数a（正如Zichao Zhang向我指出的，我们两次乘以
a
a
a，是因为预测误差
p
k
p_k
pk本身就是一个平方误差；因此，它应该由与状态值
x
k
x_k
xk相关的系数的平方进行缩放。将预测误差表示为
a
p
k
−
1
a
ap_{k-1}a
apk−1a而不是
a
2
p
k
−
1
a^2p_{k-1}
a2pk−1的原因将在Part 12一节进行说明。）：
p
k
=
a
p
k
−
1
a
p_k=ap_{k-1}a
pk=apk−1a
将上面两个等式标为红色，表示它们是卡尔曼滤波器的预测阶段。实际上卡尔曼滤波器运行过程中, 将会是一个预测/更新, 预测/更新,…重复的过程, 重复的时间步长可以按照我们的需要来设定。
Part:7 运行卡尔曼滤波器
现在, 我们已经有了运行卡尔曼滤波器所需的所有公式：
预测：
x
k
^
=
a
x
k
−
1
^
p
k
=
a
p
k
−
1
a
\hat{x_k}=a\hat{x_{k-1}} \\ {p_k}=a{p_{k-1}} a
xk^=axk−1^pk=apk−1a
更新：
g
k
=
p
k
/
(
p
k
+
r
)
x
k
^
←
x
k
^
+
g
k
(
z
k
−
x
k
^
)
p
k
←
(
1
−
g
k
)
p
k
{g_k}=p_k/({p_{k}}+r) \\ \hat{x_k}\leftarrow\hat{x_{k}}+ g_k(z_k-\hat{x_k}) \\ {p_k}\leftarrow(1-g_k){p_k}
gk=pk/(pk+r)xk^←xk^+gk(zk−xk^)pk←(1−gk)pk
注意：在更新部分的最后两行，我已经使用了箭头符号而不是标准的等号。尽管，这种用法是非传统的，但是这能够表明对
x
k
^
\hat{x_k}
xk^和
p
k
^
\hat{p_k}
pk^的更新其实是对它们（来自预测步骤的）当前值的修改，而不是根据前面的步骤定义这些值（如预测部分那样）。（事实上，如果你查看原文英文网站中本页的源代码Part 7: Running the Filter，你就会发现JavaScript实现的预测和更新和上述公式非常相似：
// Predict
xhat = a * xhat;
p = a * p * a;
// Update
g = p / (p + r);
xhat = xhat + g * (z - xhat);
p = (1 - g) * p;
感谢Marco Camurri和John Mahoney在本教程的早期版本中指出了我在使用这些方程时的不一致问题。
）
为了尝试我们的滤波器，我们需要：
- 一个观测值 z k z_k zk序列。
- 一个初始值（初始状态） x 0 ^ \hat{x_0} x0^用于状态估计。它可以是我们第一个观测值 z 0 z_0 z0。
- 一个初始值 p 0 p_0 p0作为预测误差。它不能是0，否则 p k p_k pk将会由于相乘永远保持为0。所以我们强制将其设置为1。
至于观测值，我们不需要直接对一个实际的系统进行观测（比如飞机着陆时的海拔高度变化），而是可以模拟生成观测值，这里首先假设
x
k
=
0.75
x
k
−
1
x_k=0.75x_{k-1}
xk=0.75xk−1，初始值
x
0
=
1000
x_0=1000
x0=1000，然后添加区间[-200,+200]中的随机数作为随机噪声
v
k
v_k
vk，从而模拟生成观测值序列
z
k
z_k
zk。由此也可得到，测量噪声
r
r
r为200。
下图展示了模拟生成的相关系统参数。其中，蓝色曲线表示系统的真实状态序列，红色曲线表示系统的观测值序列，初始预测误差
p
0
p_0
p0被置为1，并且以第一个观测值作为系统的初始状态
x
0
^
\hat{x_0}
x0^。
运行了我们的卡尔曼滤波器之后，得到的结果如下图所示：
可以看出，卡尔曼滤波器得到了一个观测值的平滑版本（绿色曲线），其曲线比较接近于系统的真实状态（蓝色曲线），从而实现了根据系统的当前观测值和上一个系统状态值来估计当前的系统状态。
Part:8 一个更现实的模型
回忆我们用来描述系统的两个方程：
x
k
=
a
∗
x
k
−
1
z
k
=
x
k
+
v
k
x_{k} =a*x_{k-1} \\ z_{k} = x_{k} + v_{k}
xk=a∗xk−1zk=xk+vk
其中，
x
k
x_{k}
xk是系统的当前状态，
x
k
−
1
x_{k-1}
xk−1是系统的前一状态，
a
a
a是常量，
z
k
z_k
zk是系统的当前测量值，
v
k
v_k
vk是和测量值相关的当前噪声（测量误差）。
尽管这两个方程能够适用于很多系统，但有时候它们可能不够全面。首先，我们没有考虑系统的时变控制，例如，飞行员在控制飞机飞行时，通过移动操作杆实现飞机的飞行状态变化。为了考虑这种时变控制，我们引入另一种下标变量
u
k
u_k
uk，表示飞行员正在发送给飞机的控制信号的当前值。正如前一状态量
x
k
−
1
x_{k-1}
xk−1由常量
a
a
a进行缩放（scale）一样，该控制信号也由一个常量进行缩放，我们可以称该常量为
b
b
b。由此，之前得到系统状态方程将变成：
x
k
=
a
∗
x
k
−
1
+
b
u
k
x_{k} =a*x_{k-1}+bu_k
xk=a∗xk−1+buk
通常，除了噪声以外的任意信号都可以通过一个常量进行缩放，所以第二个方程我们也可以改写为：
z
k
=
c
x
k
+
v
k
z_{k} = cx_{k} + v_{k}
zk=cxk+vk
注：为了与前面的示例保持一致，这里我选择使用变量 a a a、 b b b和 c c c作为常量，而不是更常见的 f f f、 b b b和 h h h。
Part:9 修改估计值
下面是前面得到的更加现实/更加通用的系统状态和观测方程：
x
k
=
a
∗
x
k
−
1
+
b
u
k
z
k
=
c
x
k
+
v
k
x_{k} =a*x_{k-1}+bu_k \\ z_{k} = cx_{k} + v_{k}
xk=a∗xk−1+bukzk=cxk+vk
在模型中引入的这些新成分需要预测和更新方程也进行相应的修改。
预测：
x
k
^
=
a
x
k
−
1
^
+
b
u
k
p
k
=
a
p
k
−
1
a
\hat{x_k}=a\hat{x_{k-1}}+bu_k \\ {p_k}=a{p_{k-1}} a
xk^=axk−1^+bukpk=apk−1a
更新：
g
k
=
p
k
c
/
(
c
p
k
c
+
r
)
x
k
^
←
x
k
^
+
g
k
(
z
k
−
c
x
k
^
)
p
k
←
(
1
−
g
k
c
)
p
k
{g_k}=p_kc/(c{p_{k}}c+r) \\ \hat{x_k}\leftarrow\hat{x_{k}}+ g_k(z_k-c\hat{x_k}) \\ {p_k}\leftarrow(1-g_kc){p_k}
gk=pkc/(cpkc+r)xk^←xk^+gk(zk−cxk^)pk←(1−gkc)pk
下面结合之前飞机海拔高度的例子进行一下扩展说明。下图展示了一个更长的时间过程中，添加了控制信号的飞机海拔高度结果，其中控制信号表示飞行员平稳地拉起操纵杆以升高飞机的海拔高度。和前面的例子一样，蓝色曲线表示真实信号，红色曲线表示观测信号，卡尔曼滤波信号用绿色曲线表示。
其中，
a
=
0.95
,
b
=
0.5
,
c
=
1
,
r
=
100
a=0.95,b=0.5,c=1,r=100
a=0.95,b=0.5,c=1,r=100。
Part:10 给系统添加速度
回顾最初的飞机海拔高度方程：
a
l
t
i
t
u
d
e
c
u
r
r
e
n
t
_
t
i
m
e
(
z
)
=
0.98
∗
a
l
t
i
t
u
d
e
p
r
e
v
i
o
u
s
_
t
i
m
e
altitude_{current\_time} (z) =0.98*altitude_{previous\_time}
altitudecurrent_time(z)=0.98∗altitudeprevious_time
以及更通用的形式：
x
k
=
a
∗
x
k
−
1
x_{k} =a*x_{k-1}
xk=a∗xk−1
回想一下高中时学过的数学和物理课程，这种公式看起来有点奇怪。海拔高度，毕竟是一种距离，因此有我们之前学过的公式：
d
i
s
t
a
n
c
e
=
v
e
l
o
c
i
t
y
∗
t
i
m
e
distance =velocity*time
distance=velocity∗time
我们能否综合一下考虑距离的两种不同方式？答案是可以，但是需要我们进行下面两个步骤。
首先，我们需要将当前时刻和前一时刻两个概念引入我们高中的公式，然后以离散的时间步长来考虑距离，而不是以整体的距离来考虑：
d
i
s
t
a
n
c
e
c
u
r
r
e
n
t
=
d
i
s
t
a
n
c
e
p
r
e
v
i
o
u
s
+
v
e
l
o
c
i
t
y
p
r
e
v
i
o
u
s
∗
(
t
i
m
e
c
u
r
r
e
n
t
−
t
i
m
e
p
r
e
v
i
o
u
s
)
distance_{current}=distance_{previous}+velocity_{previous}*(time_{current}-time_{previous})
distancecurrent=distanceprevious+velocityprevious∗(timecurrent−timeprevious)
换言之，我们此时所在的距离是前一时刻的距离加上这段时间内我们移动的距离。如果我们以固定的时间步长或时间间隔（1s，100ns, 6个月等）执行这种计算，那么我们可以将上式简化为：
d
i
s
t
a
n
c
e
c
u
r
r
e
n
t
=
d
i
s
t
a
n
c
e
p
r
e
v
i
o
u
s
+
v
e
l
o
c
i
t
y
p
r
e
v
i
o
u
s
∗
t
i
m
e
s
t
e
p
distance_{current}=distance_{previous}+velocity_{previous}*timestep
distancecurrent=distanceprevious+velocityprevious∗timestep
这个等式令我们更接近于之前得到的通用形式：
x
k
=
a
∗
x
k
−
1
x_{k} =a*x_{k-1}
xk=a∗xk−1
但是看上去我们仍然具有两种不同的系统：一种包含一个简单的乘积，而另一种包含一个乘积和一个和。要为上述两种方程提出一个通用方程，这就引出了我们的第二步：线性代数。
下面请看本教程的第三部分:扩展卡尔曼滤波新手教程（三）----中文版.
588
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
