第11章 The Extended Kalman Filter
11.1 线性化卡尔曼滤波
- Linear Kalman Filter, 状态方程
x
˙
=
A
x
+
B
u
+
w
z
=
H
x
+
v
\begin{aligned} \dot{\pmb{x}} &= \pmb{Ax}+\pmb{Bu}+\pmb{w} \\ \pmb{z} &= \pmb{Hx}+\pmb{v} \end{aligned}
xxx˙zzz=AxAxAx+BuBuBu+www=HxHxHx+vvv
目前
成都创新互联已为上1000家的企业提供了网站建设、域名、
虚拟主机、网站托管、
服务器托管、企业网站设计、
昌都网站维护等服务，公司将坚持客户导向、应用为本的策略，正道将秉承"和谐、参与、激情"的文化，与客户和合作伙伴齐心协力一起成长，共同发展。
- Extended Kalman Filter, 状态方程
x
=
f
(
x
,
u
)
+
w
z
=
h
(
x
)
+
ν
\begin{aligned} \pmb{x} &= \pmb{f}(\pmb{x},\pmb{u})+\pmb{w}\\ \pmb{z} &= \pmb{h}(\pmb{x})+\pmb{\nu} \end{aligned}
xxxzzz=fff(xxx,uuu)+www=hhh(xxx)+ννν
F
=
∂
f
(
x
t
,
u
t
)
∂
x
∣
x
t
,
u
t
H
=
∂
h
(
x
ˉ
t
)
∂
x
ˉ
∣
x
ˉ
t
\begin{aligned} \mathbf F &= {\frac{\partial{f(\mathbf x_t, \mathbf u_t)}}{\partial{\mathbf x}}}\biggr|_{{\mathbf x_t},{\mathbf u_t}} \\ \mathbf H &= \frac{\partial{h(\bar{\mathbf x}_t)}}{\partial{\bar{\mathbf x}}}\biggr|_{\bar{\mathbf x}_t} \end{aligned}
FH=∂x∂f(xt,ut)∣∣∣∣xt,ut=∂xˉ∂h(xˉt)∣∣∣∣xˉt
linear Kalman filter
EKF
F
=
∂
f
(
x
t
,
u
t
)
∂
x
∣
x
t
,
u
t
x
ˉ
=
F
x
+
B
u
x
ˉ
=
f
(
x
,
u
)
P
ˉ
=
F
P
F
T
+
Q
P
ˉ
=
F
P
F
T
+
Q
H
=
∂
h
(
x
ˉ
t
)
∂
x
ˉ
∣
x
ˉ
t
y
=
z
−
H
x
ˉ
y
=
z
−
h
(
x
ˉ
)
K
=
P
ˉ
H
T
(
H
P
ˉ
H
T
+
R
)
−
1
K
=
P
ˉ
H
T
(
H
P
ˉ
H
T
+
R
)
−
1
x
=
x
ˉ
+
K
y
x
=
x
ˉ
+
K
y
P
=
(
I
−
K
H
)
P
ˉ
P
=
(
I
−
K
H
)
P
ˉ
\begin{array}{l|l} \text{linear Kalman filter} & \text{EKF} \\ \hline & \boxed{\mathbf F = {\frac{\partial{f(\mathbf x_t, \mathbf u_t)}}{\partial{\mathbf x}}}\biggr|_{{\mathbf x_t},{\mathbf u_t}}} \\ \mathbf{\bar x} = \mathbf{Fx} + \mathbf{Bu} & \boxed{\mathbf{\bar x} = f(\mathbf x, \mathbf u)} \\ \mathbf{\bar P} = \mathbf{FPF}^\mathsf{T}+\mathbf Q & \mathbf{\bar P} = \mathbf{FPF}^\mathsf{T}+\mathbf Q \\ \hline & \boxed{\mathbf H = \frac{\partial{h(\bar{\mathbf x}_t)}}{\partial{\bar{\mathbf x}}}\biggr|_{\bar{\mathbf x}_t}} \\ \textbf{y} = \mathbf z - \mathbf{H \bar{x}} & \textbf{y} = \mathbf z - \boxed{h(\bar{x})}\\ \mathbf{K} = \mathbf{\bar{P}H}^\mathsf{T} (\mathbf{H\bar{P}H}^\mathsf{T} + \mathbf R)^{-1} & \mathbf{K} = \mathbf{\bar{P}H}^\mathsf{T} (\mathbf{H\bar{P}H}^\mathsf{T} + \mathbf R)^{-1} \\ \mathbf x=\mathbf{\bar{x}} +\mathbf{K\textbf{y}} & \mathbf x=\mathbf{\bar{x}} +\mathbf{K\textbf{y}} \\ \mathbf P= (\mathbf{I}-\mathbf{KH})\mathbf{\bar{P}} & \mathbf P= (\mathbf{I}-\mathbf{KH})\mathbf{\bar{P}} \end{array}
linear Kalman filterxˉ=Fx+BuPˉ=FPFT+Qy=z−HxˉK=PˉHT(HPˉHT+R)−1x=xˉ+KyP=(I−KH)PˉEKFF=∂x∂f(xt,ut)∣∣∣∣xt,utxˉ=f(x,u)Pˉ=FPFT+QH=∂xˉ∂h(xˉt)∣∣∣∣xˉty=z−h(xˉ)K=PˉHT(HPˉHT+R)−1x=xˉ+KyP=(I−KH)Pˉ
11.2 实例
背景：1）考虑二维平面；2）Aircraft只有水平方向的速度；3）Aircraft高度假设不变；4）Radar随着距离的变大，精度减低
class Radar:
"""模拟雷达传感器(二维平面)"""
def __init__(self, dt, pos, vel, alt):
"""
Parameters
----------
dt : double
采样时间
pos : double
水平距离
vel : double
水平速度
alt : double
垂直高度
Returns
-------
None.
"""
# 初始化Radar
self.dt = dt
self.pos = pos
self.vel = vel
self.alt = alt
def get_range(self):
"""得到物体当前运动信息"""
# 添加一些噪声
self.vel = self.vel + .1*randn()
self.alt = self.alt + .1*randn()
# 从上一刻的位置获得当前时刻的位置
self.pos = self.pos + self.vel*self.dt
# 假设雷达的精度随着距离变远而越来越低
err = self.pos * (0.05*randn())
dist = math.sqrt(self.pos**2 + self.alt**2) + err
return dist
下面说明系统的状态方程和测量方程，首先得确定系统的状态变量
x
=
(
x
0
x
1
x
2
)
T
\pmb{x} = \begin{pmatrix} x_0 & x_1 & x_2 \end{pmatrix}^T
xxx=(x0x1x2)T
x
0
{x}_0
x0：水平距离。
x
1
{x}_1
x1：水平速度。
x
2
{x}_2
x2：垂直高度。
- 系统状态方程：（需要注意，速度只有水平方向，所以
x
˙
0
=
x
1
\dot{x}_0=x_1
x˙0=x1）
[
x
˙
0
x
˙
1
x
˙
2
]
=
[
0
1
0
0
0
0
0
0
0
]
[
x
0
x
1
x
2
]
+
w
\begin{bmatrix} \dot{x}_0 \\ \dot{x}_1 \\ \dot{x}_2 \\ \end{bmatrix} =\begin{bmatrix} 0 & 1 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ \end{bmatrix} \begin{bmatrix} x_0 \\ x_1 \\ x_2 \\ \end{bmatrix} + \pmb{w}
⎣⎡x˙0x˙1x˙2⎦⎤=⎣⎡000100000⎦⎤⎣⎡x0x1x2⎦⎤+www
系统的状态方程是连续的线性的，将其离散化（对确定性部分）：
F
=
e
A
d
t
≈
I
+
A
d
t
\begin{aligned} \pmb{F} &= e^{\pmb{A}dt} \\ &\approx \pmb{I} + \pmb{A}dt \end{aligned}
FFF=eAAAdt≈III+AAAdt
所以离散化后的状态方程有：(其实这里还有一个问题，
w
\pmb{w}
www变成什么了？）
x
(
k
)
=
[
1
d
t
0
0
1
0
0
0
1
]
x
(
k
−
1
)
\pmb{x}(k)= \begin{bmatrix} 1 & dt & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} \pmb{x}(k-1)
xxx(k)=⎣⎡100dt10001⎦⎤xxx(k−1)
- 测量方程
z
=
h
(
x
)
+
v
=
x
0
2
+
x
2
2
+
v
\begin{aligned} \pmb{z} &= \pmb{h}(\pmb{x}) + \pmb{v} \\ &= \sqrt{x_{0}^{2}+x_{2}^{2}} + \pmb{v} \\ \end{aligned}
zzz=hhh(xxx)+vvv=x02+x22
+vvv
离散化后，测量方程为；
z
k
=
[
x
0
x
0
2
+
x
2
2
0
x
2
x
0
2
+
x
2
2
]
x
k
\pmb{z}_k= \begin{bmatrix} \frac{x_0}{x_0^2+x_2^2} & 0 & \frac{x_2}{x_0^2+x_2^2} \end{bmatrix} \pmb{x}_k
zzzk=[x02+x22x00x02+x22x2]xxxk
这里有几点需要注意：
- 特别是随机噪声
Q
,
R
\pmb{Q},\pmb{R}
QQQ,RRR，需要根据实际的情况进行不断的调整
- 初始值的设定具有很大的随机性，
x
0
,
P
0
\pmb{x}_0,\pmb{P}_0
xxx0,PPP0几乎没有什么影响。
关于这一点可以参考一篇文献，严恭敏老师的《传统组合导航中的实用Kalman滤波技术评述》，给出了很多实用的建议：1）只要滤波器是渐近稳定，初值不会影响估计的最终结果（和系统的可观性有一定的关系）；2）去精确的离散化噪声的协方差矩阵，还不如增加采样频率所提高的精度多。
- 仿真结果（及其调试的过程记录）
- 协方差矩阵的更新公式，在代码中的实现出现了错误。/_ \
- 然后改到正确的公式后，它竟然还是发散的X_X
问题分析：1）在初始值的估计值还是没有偏差的情况下进行的。。2）
解决办法：1）查看kalman滤波是否正确表达了；2）将初始值调为有一定偏差的，看是否有其它的问题；3)增加计算的时间后，又出现了NaN的数据，看来电脑的计算还是没有搞清楚！
结果：发现对于Numpy的数值的维度有点混乱，导致矩阵的计算出现了些许问题。现在已经有点晕了，需要重新再来一遍（遇到了一点问题，于是就又想逃避了哈？？还是先吃饭吧）
(python numpy) np.array.shape 中 (3,)、(3,1)、(1,3)的区别_我是一颗棒棒糖的博客-博客_np.array.shape
这里在强调一下，需要注意一下这个的运算过程，看看是否对于np.dot有什么误解呢。（突然好想用matlab啊，这个python是真的┭┮﹏┭┮）
numpy还有数组和矩阵的区别，我是真的醉醉的呢啊，下面我该怎么办？(就用数组类型了，现在就是要搞懂数组的运算，当然刚才维数这里也搞错了一点问题，继续加深理解呗）
- 经过仔细的调整，主要就是numpy的narray的运算进行了详细的对比，现在的结果稍微好了一点点呢！（这里对于状态的初始值也进行了调整和容易看出来的）
现在这里还有一个很明显的问题，速度的收敛速度好像有点慢，而且最后对于高度的估计有发散的趋势吗？（延长一点时间看看？）
然后它它，就直接起飞！（分析原因，因为随着距离的增加，radar的噪声逐渐增大的，然后就不能正确的估计啦？后面调试后确实是的）
一个这个程序，竟然调了两天，我？？？
分析原因：
- EKF还是不太熟悉，连公式都打错
- 第一次用Python来写，对于一些基本的运算不太了解，导致很多错误的都是计算的形式有问题
- 遇到问题的心态要好好调整，哪里不会就点哪个（人生苦短，不用什么都要学透）
参考资料：
Kalman-and-Bayesian-Filters-in-Python
你是否还在寻找稳定的海外服务器提供商？创新互联www.cdcxhl.cn海外机房具备T级流量清洗系统配攻击溯源，准确流量调度确保服务器高可用性，企业级服务器适合批量采购，新人活动首月15元起，快前往官网查看详情吧
当前题目：第11章TheExtendedKalmanFilter-创新互联
标题网址：
http://cdkjz.cn/article/cssdie.html
