Kalman Filter
Kalman Filter
Optimal Recursive Data Processing Algorithm
不确定性:
1、不存在完美的数学模型
2、系统的扰动不可控,也很难建模
3、测量传感器存在误差
Recursive Algorithm
第 K 次测量结果
z
k
z_k
zk
通过样本均值来估计真实值
估计值
x
^
k
=
1
k
(
z
1
+
z
2
+
.
.
.
+
z
k
)
=
1
k
(
z
1
+
z
2
+
.
.
.
+
z
k
−
1
)
+
1
k
z
k
=
k
−
1
k
x
^
k
−
1
+
1
k
z
k
=
x
^
k
−
1
+
1
k
(
z
k
−
x
^
k
−
1
)
\hat x_k = \frac{1}{k}(z_1+z_2+...+z_k) = \frac{1}{k}(z_1+z_2+...+z_{k-1})+\frac{1}{k}z_k = \frac{k-1}{k}\hat x_{k-1}+\frac{1}{k}z_k=\hat x_{k-1}+\frac{1}{k}(z_k-\hat x_{k-1})
x^k=k1(z1+z2+...+zk)=k1(z1+z2+...+zk−1)+k1zk=kk−1x^k−1+k1zk=x^k−1+k1(zk−x^k−1)
k
→
∞
,
1
k
→
0
,
x
^
k
=
x
^
k
−
1
k \to \infty,\frac{1}{k} \to 0,\hat x_k=\hat x_{k-1}
k→∞,k1→0,x^k=x^k−1 测量结果不重要
k
→
0
,
1
k
→
∞
k \to 0,\frac{1}{k} \to \infty
k→0,k1→∞ 测量值
z
k
z_k
zk作用较大
令
k
k
=
1
k
k_k=\frac{1}{k}
kk=k1,其中
k
k
k_k
kk为Kalman Gain
x
^
k
=
x
^
k
−
1
+
k
k
(
z
k
−
x
^
k
−
1
)
\hat x_k = \hat x_{k-1}+k_k(z_k-\hat x_{k-1})
x^k=x^k−1+kk(zk−x^k−1)
当前估计值 = 上次估计值 + 系数 *(当前测量值-上次估计值)
估计误差
e
E
S
T
e_{EST}
eEST
测量误差
e
M
E
A
e_{MEA}
eMEA
Klaman Gain
k
k
=
e
E
S
T
k
−
1
e
E
S
T
k
−
1
+
e
M
E
A
k
k_k = \frac{e_{EST_{k-1}}}{e_{EST_{k-1}}+e_{MEA_k}}
kk=eESTk−1+eMEAkeESTk−1
①
e
E
S
T
k
−
1
≫
e
M
E
A
k
,
k
k
→
1
,
x
^
k
=
z
k
e_{EST_{k-1}} \gg e_{MEA_k}, k_k\to1, \hat x_k=z_k
eESTk−1≫eMEAk,kk→1,x^k=zk 估计误差大,估计值取测量值
②
e
E
S
T
k
−
1
≪
e
M
E
A
k
,
k
k
→
0
,
x
^
k
=
x
^
k
−
1
e_{EST_{k-1}} \ll e_{MEA_k}, k_k\to0, \hat x_k=\hat x_{k-1}
eESTk−1≪eMEAk,kk→0,x^k=x^k−1 测量误差大,估计值取上次估计值
迭代过程
STEP1:计算 Kalman Gain
k
k
=
e
E
S
T
k
−
1
e
E
S
T
k
−
1
+
e
M
E
A
k
k_k = \frac{e_{EST_{k-1}}}{e_{EST_{k-1}}+e_{MEA_k}}
kk=eESTk−1+eMEAkeESTk−1
STEP2:计算
x
^
k
=
x
^
k
−
1
+
k
k
(
z
k
−
x
^
k
−
1
)
\hat x_k = \hat x_{k-1} + k_k(z_k-\hat x_{k-1})
x^k=x^k−1+kk(zk−x^k−1)
STEP3:更新
e
E
S
T
k
=
(
1
−
k
k
)
e
E
S
T
k
−
1
e_{EST_{k}}=(1-k_k)e_{EST_{k-1}}
eESTk=(1−kk)eESTk−1
数学基础
正态分布和6-Sigma
通过测量获取多组数据,用样本均值代替期望,样本方差代替方差
样本方差
S
2
=
1
n
∑
i
=
1
n
(
x
i
−
x
^
)
2
S^2 = \frac{1}{n}\sum_{i=1}^{n}{(x_i-{\hat x})^2}
S2=n1∑i=1n(xi−x^)2
样本标准差
S
=
1
n
∑
i
=
1
n
(
x
i
−
x
^
)
2
S= \sqrt{ \frac{1}{n}\sum_{i=1}^{n}{(x_i-{\hat x})^2} }
S=n1∑i=1n(xi−x^)2
理论正态分布
实际修正后的对应表格
σ \sigma σ | 产出百分比 | 瑕疵百分比 |
---|---|---|
3 | 93.3 % | 6.7% |
5 | 99.977% | 0.023 % |
6 | 99.99966% | 0.00034% |
生产设备
每日生产量 | 3 σ \sigma σ | 5 σ \sigma σ | 6 σ \sigma σ |
---|---|---|---|
200 | 1天13次 | 21天1次 | 4年1次 |
false dismissal 漏检
将某些不合格视为合格
false alarm 假警报
将某些合格视为不合格
Data Fusion
Measurement:
z
1
=
30
g
z_1=30g
z1=30g Standard Deviation:
σ
1
=
2
g
\sigma_1=2g
σ1=2g
Measurement:
z
2
=
32
g
z_2=32g
z2=32g Standard Deviation:
σ
1
=
4
g
\sigma_1=4g
σ1=4g
两者均遵从 Natural Distribution
估计真实值
z
^
\hat z
z^
z
^
=
z
1
+
k
(
z
2
−
z
1
)
,
k
∈
[
0
,
1
]
\hat z=z_1+k(z_2-z_1),k \in[0, 1]
z^=z1+k(z2−z1),k∈[0,1]
求 k 使得
σ
z
^
\sigma_{\hat z}
σz^最小
σ
z
^
2
=
V
a
r
(
z
1
+
k
(
z
2
−
z
1
)
)
=
V
a
r
[
(
1
−
k
)
z
1
+
k
z
2
]
=
V
a
r
[
(
1
−
k
)
z
1
]
+
V
a
r
(
k
z
2
)
=
(
1
−
k
)
2
σ
1
2
+
k
2
σ
2
2
\sigma_{\hat z}^2=Var(z_1+k(z_2-z_1)) = Var[(1-k)z_1+kz_2]=Var[(1-k)z_1]+Var(kz_2)=(1-k)^2\sigma_1^2+k^2\sigma_2^2
σz^2=Var(z1+k(z2−z1))=Var[(1−k)z1+kz2]=Var[(1−k)z1]+Var(kz2)=(1−k)2σ12+k2σ22
d
σ
z
^
2
d
k
=
−
2
(
1
−
k
)
σ
1
2
+
2
k
σ
2
2
=
0
\frac{d\sigma_{\hat z}^2}{dk}=-2(1-k)\sigma_1^2+2k\sigma_2^2=0
dkdσz^2=−2(1−k)σ12+2kσ22=0
k
=
σ
1
2
σ
1
2
+
σ
2
2
k=\frac{\sigma_1^2}{\sigma_1^2+\sigma_2^2}
k=σ12+σ22σ12
将数据带入
k
=
σ
1
2
σ
1
2
+
σ
2
2
=
4
4
+
16
=
0.2
k=\frac{\sigma_1^2}{\sigma_1^2+\sigma_2^2}=\frac{4}{4+16}=0.2
k=σ12+σ22σ12=4+164=0.2
z
^
=
30
+
0.2
∗
(
32
−
30
)
=
30.4
\hat z=30+0.2*(32-30)=30.4
z^=30+0.2∗(32−30)=30.4 最优解
σ
z
^
2
=
0.
8
2
∗
4
+
0.
2
2
∗
16
=
3.2
\sigma_{\hat z}^2=0.8^2*4+0.2^2*16=3.2
σz^2=0.82∗4+0.22∗16=3.2
Covariance Matrix
将方差、协方差在一个矩阵中表现出来,体现变量间的联动关系
球员 | 身高(x) | 体重(y) | 年龄(z) |
---|---|---|---|
瓦尔迪 | 179 | 74 | 33 |
奥巴梅扬 | 187 | 80 | 31 |
萨拉赫 | 175 | 71 | 28 |
平均 | 180.3 | 75 | 30.7 |
方差
σ
x
2
=
1
3
[
(
179
−
180.3
)
2
+
(
187
−
180.3
)
2
+
(
175
−
180.3
)
2
]
=
24.89
\sigma_x^2=\frac{1}{3}[(179-180.3)^2+(187-180.3)^2+(175-180.3)^2]=24.89
σx2=31[(179−180.3)2+(187−180.3)2+(175−180.3)2]=24.89
σ
y
2
=
14
\sigma_y^2 = 14
σy2=14
σ
z
2
=
4.22
\sigma_z^2=4.22
σz2=4.22
协方差
σ
x
σ
y
=
1
3
[
(
179
−
180.3
)
(
74
−
75
)
+
(
187
−
180.3
)
(
80
−
75
)
+
(
175
−
180.3
)
(
71
−
75
)
]
=
18.7
=
σ
y
σ
x
\sigma_x \sigma_y=\frac{1}{3}[(179-180.3)(74-75)+(187-180.3)(80-75)+(175-180.3)(71-75)]=18.7=\sigma_y\sigma_x
σxσy=31[(179−180.3)(74−75)+(187−180.3)(80−75)+(175−180.3)(71−75)]=18.7=σyσx
σ
x
σ
z
=
4.4
=
σ
z
σ
x
\sigma_x\sigma_z=4.4=\sigma_z\sigma_x
σxσz=4.4=σzσx
σ
y
σ
z
=
3.3
=
σ
z
σ
y
\sigma_y\sigma_z=3.3=\sigma_z\sigma_y
σyσz=3.3=σzσy
Covariance matrix P = [ σ x 2 σ x σ y σ x σ z σ y σ x σ y 2 σ y σ z σ z σ x σ z σ y σ z 2 ] P= \left[ \begin{matrix} \sigma_x^2 & \sigma_x \sigma_y & \sigma_x \sigma_z \\ \sigma_y \sigma_x & \sigma_y^2 & \sigma_y \sigma_z \\ \sigma_z\sigma_x & \sigma_z\sigma_y & \sigma_z^2 \end{matrix} \right] P= σx2σyσxσzσxσxσyσy2σzσyσxσzσyσzσz2
过度矩阵 a = [ x 1 y 1 z 1 x 2 y 2 z 2 x 3 y 3 z 3 ] − 1 3 [ 1 1 1 1 1 1 1 1 1 ] [ x 1 y 1 z 1 x 2 y 2 z 2 x 3 y 3 z 3 ] 过度矩阵 a=\left[\begin{matrix} x_1 & y_1 & z_1 \\ x_2 & y_2 & z_2 \\ x_3 & y_3 & z_3 \end{matrix}\right] - \frac{1}{3} \left[\begin{matrix} 1 & 1 & 1 \\ 1 & 1 & 1 \\ 1 & 1 & 1 \end{matrix}\right] \left[\begin{matrix} x_1 & y_1 & z_1 \\ x_2 & y_2 & z_2 \\ x_3 & y_3 & z_3 \end{matrix}\right] 过度矩阵a= x1x2x3y1y2y3z1z2z3 −31 111111111 x1x2x3y1y2y3z1z2z3
P = 1 3 a T a P=\frac{1}{3}a^Ta P=31aTa
σ x σ y \sigma_x \sigma_y σxσy表示协方差,不是两个标准差相乘
State Space Representation
m
x
¨
=
F
−
k
x
−
b
x
˙
m \ddot x=F-kx-b\dot x
mx¨=F−kx−bx˙
F 为 Input,记为u
m
x
¨
=
u
−
k
x
−
b
x
˙
m \ddot x=u-kx-b\dot x
mx¨=u−kx−bx˙
取 State Variable
x
1
=
x
x_1=x
x1=x
x
2
=
x
˙
x_2=\dot x
x2=x˙
x
˙
1
=
x
˙
=
x
2
\dot x_1 = \dot x = x_2
x˙1=x˙=x2
x
˙
2
=
x
¨
=
1
m
u
−
k
m
x
−
b
m
x
˙
=
1
m
u
−
k
m
x
1
−
b
m
x
2
\dot x_2 = \ddot x=\frac{1}{m}u-\frac{k}{m}x-\frac{b}{m}\dot x=\frac{1}{m}u-\frac{k}{m}x_1-\frac{b}{m}x_2
x˙2=x¨=m1u−mkx−mbx˙=m1u−mkx1−mbx2
( x ˙ 1 x ˙ 2 ) = ( 0 1 − k m − b m ) ( x 1 x 2 ) + ( 0 1 m ) u \left(\begin{matrix} \dot x_1 \\ \dot x_2 \end{matrix}\right) = \left(\begin{matrix} 0 & 1 \\ -\frac{k}{m} & -\frac{b}{m} \end{matrix}\right) \left(\begin{matrix} x_1 \\ x_2 \end{matrix}\right)+ \left(\begin{matrix} 0 \\ \frac{1}{m} \end{matrix}\right)u (x˙1x˙2)=(0−mk1−mb)(x1x2)+(0m1)u
记作 随时间变化 X ˙ ( t ) = A X ( t ) + B U ( t ) \dot X(t) = AX(t) + BU(t) X˙(t)=AX(t)+BU(t)
测量量Measurement
z
1
=
x
=
x
1
位置
z_1=x=x_1 位置
z1=x=x1位置
z
2
=
x
˙
=
x
2
速度
z_2=\dot x=x_2 速度
z2=x˙=x2速度
( z 1 z 2 ) = ( 1 0 0 1 ) ( x 1 x 2 ) \left(\begin{matrix} z_1 \\ z_2 \end{matrix}\right) = \left(\begin{matrix} 1 & 0 \\ 0 & 1 \end{matrix}\right) \left(\begin{matrix} x_1 \\ x_2 \end{matrix}\right) (z1z2)=(1001)(x1x2)
记作 Z(t) = HX(t)
整个过程充满不确定性,故状态空间方程离散化表达式为
计算值/估计值
X
k
=
A
X
k
−
1
+
B
U
k
+
W
k
−
1
X_k = AX_{k-1}+BU_k+W_{k-1}
Xk=AXk−1+BUk+Wk−1
测量值
Z
k
=
H
X
k
+
V
k
Z_k=HX_k + V_k
Zk=HXk+Vk
W
k
−
1
W_{k-1}
Wk−1 记作过程噪音 Process Noise
V
k
V_k
Vk 记作测量噪音 Measurement Noise
可通过前面介绍的数据融合,将估计值与测量值融合,得到更加可信赖的目标估计值
离散化推导
通过欧拉法(前向差分)将状态空间方程离散化
X
˙
=
X
k
+
1
−
X
k
T
=
A
X
k
+
B
U
k
\dot X = \frac{X_{k+1}-X_{k}}{T}=AX_k+BU_k
X˙=TXk+1−Xk=AXk+BUk
X
k
+
1
=
(
T
A
+
I
)
X
k
+
T
B
U
k
X_{k+1} = (TA+I)X_{k}+TBU_{k}
Xk+1=(TA+I)Xk+TBUk
记作
X
k
+
1
=
Φ
X
k
+
G
U
k
X_{k+1} = \Phi X_{k} + GU_{k}
Xk+1=ΦXk+GUk
故 X k = Φ X k − 1 + G U k − 1 + W k − 1 X_k = \Phi X_{k-1} + GU_{k-1}+W_{k-1} Xk=ΦXk−1+GUk−1+Wk−1
linearization
Taylor Series
线性系统 linear system ⇔ \Leftrightarrow ⇔ 叠加原理 superposition
x
˙
=
f
(
x
)
\dot x =f(x)
x˙=f(x)
①
x
1
,
x
2
x_1, x_2
x1,x2是解
②
x
3
=
k
1
x
1
+
k
2
x
2
(
k
1
,
k
2
为
c
o
n
s
t
a
n
t
)
x_3=k_1x_1+k_2x_2(k_1,k_2为constant)
x3=k1x1+k2x2(k1,k2为constant)
③
x
3
x_3
x3是解
线性化:Taylor Series
是在某一点附近的线性化,而不是全局线性化
f
(
x
)
=
f
(
x
0
)
+
f
′
(
x
0
)
1
!
(
x
−
x
0
)
+
f
′
′
(
x
0
)
2
!
(
x
−
x
0
)
2
+
.
.
.
+
f
(
n
)
(
x
0
)
n
!
(
x
−
x
0
)
n
f(x)=f(x_0)+\frac{f'(x_0)}{1!}(x-x_0)+\frac{f''(x_0)}{2!}(x-x_0)^2+...+\frac{f^{(n)}(x_0)}{n!}(x-x_0)^n
f(x)=f(x0)+1!f′(x0)(x−x0)+2!f′′(x0)(x−x0)2+...+n!f(n)(x0)(x−x0)n
若
x
−
x
0
→
0
x-x_0 \rightarrow 0
x−x0→0,则
(
x
−
x
0
)
2
→
0
(x-x_0)^2 \rightarrow 0
(x−x0)2→0,
(
x
−
x
0
)
n
→
0
(x-x_0)^n \rightarrow 0
(x−x0)n→0
故
f
(
x
)
=
f
(
x
0
)
+
f
′
(
x
0
)
(
x
−
x
0
)
f(x) = f(x_0) + f'(x_0)(x-x_0)
f(x)=f(x0)+f′(x0)(x−x0) 即
f
(
x
)
=
k
2
x
+
b
f(x) = k_2x+b
f(x)=k2x+b
平衡点
x
¨
+
x
˙
+
1
x
=
1
\ddot x + \dot x + \frac{1}{x} =1
x¨+x˙+x1=1 在平衡点(Fixed Point)附近线性化
令
x
¨
=
x
˙
=
0
\ddot x=\dot x=0
x¨=x˙=0
则平衡点
x
0
=
1
则平衡点x_0=1
则平衡点x0=1
around
x
0
:
x
δ
=
x
0
+
x
d
x_0:x_\delta = x_0+x_d
x0:xδ=x0+xd
x
¨
δ
+
x
˙
δ
+
1
x
δ
=
1
\ddot x_\delta+\dot x_\delta+\frac{1}{x_\delta}=1
x¨δ+x˙δ+xδ1=1
1
x
δ
的线性化
1
x
δ
=
1
x
0
−
1
x
0
2
(
x
δ
−
x
0
)
=
1
−
x
d
\frac{1}{x_\delta}的线性化\frac{1}{x_\delta}=\frac{1}{x_0}-\frac{1}{x_0^2}(x_\delta-x_0)=1-x_d
xδ1的线性化xδ1=x01−x021(xδ−x0)=1−xd
故
x
¨
d
+
x
˙
d
+
1
−
x
d
=
1
\ddot x_d+\dot x_d+1-x_d=1
x¨d+x˙d+1−xd=1
即
x
¨
d
+
x
˙
d
−
x
d
=
0
\ddot x_d+\dot x_d-x_d=0
x¨d+x˙d−xd=0
2-D
x
˙
1
=
f
1
(
x
1
,
x
2
)
\dot x_1=f_1(x_1,x_2)
x˙1=f1(x1,x2)
x
˙
2
=
f
2
(
x
1
,
x
2
)
\dot x_2=f_2(x_1,x_2)
x˙2=f2(x1,x2)
[ x ˙ 1 d x ˙ 2 d ] = [ ∂ f 1 ∂ x 1 ∂ f 1 ∂ x 2 ∂ f 2 ∂ x 1 ∂ f 2 ∂ x 2 ] ∣ x = x 0 [ x 1 d x 2 d ] \left[\begin{matrix}\dot x_{1d} \\ \dot x_{2d}\end{matrix}\right]= \left[\begin{matrix}\frac{\partial f_1}{\partial x_1} & \frac{\partial f_1}{\partial x_2}\\ \frac{\partial f_2}{\partial x_1} & \frac{\partial f_2}{\partial x_2} \end{matrix}\right]_{|x=x_0} \left[\begin{matrix} x_{1d} \\ x_{2d}\end{matrix}\right] [x˙1dx˙2d]=[∂x1∂f1∂x1∂f2∂x2∂f1∂x2∂f2]∣x=x0[x1dx2d]
x
¨
+
x
˙
+
1
x
=
1
\ddot x + \dot x + \frac{1}{x} =1
x¨+x˙+x1=1
Let
x
1
=
x
,
x
2
=
x
˙
x_1=x,x_2=\dot x
x1=x,x2=x˙
x
˙
1
=
x
˙
=
x
2
x
˙
2
=
x
¨
=
−
x
˙
−
1
x
+
1
=
−
x
2
−
1
x
1
+
1
\dot x_1=\dot x=x_2\\ \dot x_2=\ddot x=-\dot x-\frac{1}{x}+1=-x_2-\frac{1}{x_1}+1
x˙1=x˙=x2x˙2=x¨=−x˙−x1+1=−x2−x11+1
令
x
˙
1
=
x
˙
2
=
0
\dot x_1=\dot x_2=0
x˙1=x˙2=0,平衡点
x
10
=
1
,
x
20
=
0
x_{10}=1, x_{20}=0
x10=1,x20=0
[
x
˙
1
d
x
˙
2
d
]
=
[
0
1
1
−
1
]
[
x
1
d
x
2
d
]
\left[\begin{matrix}\dot x_{1d} \\ \dot x_{2d}\end{matrix}\right]= \left[\begin{matrix}0 & 1 \\ 1 & -1 \end{matrix}\right] \left[\begin{matrix} x_{1d} \\ x_{2d}\end{matrix}\right]
[x˙1dx˙2d]=[011−1][x1dx2d]
即
x
˙
1
d
=
x
2
d
x
˙
2
d
=
x
1
d
−
x
2
d
x
¨
d
=
x
d
−
x
˙
d
⇒
x
¨
d
+
x
˙
d
−
x
d
=
0
\dot x_{1d}=x_{2d}\\ \dot x_{2d}=x_{1d}-x_{2d}\\ \ddot x_d=x_d-\dot x_d \Rightarrow \ddot x_d + \dot x_d - x_d=0
x˙1d=x2dx˙2d=x1d−x2dx¨d=xd−x˙d⇒x¨d+x˙d−xd=0
Summary
1-D
f
(
x
)
=
f
(
x
0
)
+
f
′
(
x
0
)
(
x
−
x
0
)
,
x
−
x
0
→
0
f(x)=f(x_0)+f'(x_0)(x-x_0), x-x_0 \rightarrow 0
f(x)=f(x0)+f′(x0)(x−x0),x−x0→0
2-D
[
x
˙
1
d
x
˙
2
d
]
=
[
∂
f
1
∂
x
1
∂
f
1
∂
x
2
∂
f
2
∂
x
1
∂
f
2
∂
x
2
]
∣
x
=
x
0
→
F
i
x
e
d
P
o
i
n
t
[
x
1
d
x
2
d
]
\left[\begin{matrix}\dot x_{1d} \\ \dot x_{2d}\end{matrix}\right]= \left[\begin{matrix}\frac{\partial f_1}{\partial x_1} & \frac{\partial f_1}{\partial x_2}\\ \frac{\partial f_2}{\partial x_1} & \frac{\partial f_2}{\partial x_2} \end{matrix}\right]_{|x=x_0 \rightarrow Fixed \ Point} \left[\begin{matrix} x_{1d} \\ x_{2d}\end{matrix}\right]
[x˙1dx˙2d]=[∂x1∂f1∂x1∂f2∂x2∂f1∂x2∂f2]∣x=x0→Fixed Point[x1dx2d]
Step by Step Derivation of Kalman Gain
X k = A X k − 1 + B U k − 1 + W k − 1 Z k = H X k + V k X_k = AX_{k-1} + BU_{k-1} + W_{k-1}\\ Z_k=HX_k+V_k Xk=AXk−1+BUk−1+Wk−1Zk=HXk+Vk
A为状态矩阵 B为控制矩阵
P(W) ~ N(0, Q) ,W为过程噪声
Q 为协方差矩阵,
Q
=
E
[
W
W
T
]
Q=E[WW^T]
Q=E[WWT]
P(v) ~ N(0, R),V为测量噪声
R 为协方差矩阵,
R
=
E
[
V
V
T
]
R=E[VV^T]
R=E[VVT]
先验估计值
X
^
k
−
=
A
X
k
−
1
+
B
U
k
−
1
\hat X_k^- = AX_{k-1}+BU_{k-1}
X^k−=AXk−1+BUk−1
测量值预测值
Z
k
=
H
X
k
⟹
X
^
k
M
E
A
=
H
−
1
Z
k
Z_k=HX_k \Longrightarrow \hat X_{kMEA}=H^{-1}Z_k
Zk=HXk⟹X^kMEA=H−1Zk
X
^
k
=
X
^
k
−
+
G
(
H
−
1
Z
k
−
X
^
k
−
)
,
G
∈
[
0
,
1
]
\hat X_k = \hat X_k^- + G(H^{-1}Z_k-\hat X_k^{-}), G\in[0, 1]
X^k=X^k−+G(H−1Zk−X^k−),G∈[0,1]
令
G
=
K
k
H
G=K_kH
G=KkH,带入上式可得:
X
^
k
=
X
^
k
−
+
K
k
(
Z
k
−
H
X
^
k
−
)
,
K
k
∈
[
0
,
H
−
1
]
\hat X_k = \hat X_k^{-} + K_k(Z_k-H\hat X_k^-), K_k \in [0, H^{-1}]
X^k=X^k−+Kk(Zk−HX^k−),Kk∈[0,H−1]
目标: 寻找 K k ,使得 X ^ k → X k ,即使得 V a r ( e ^ k ) 最小 K_k,使得\hat X_k \to X_k,即使得Var(\hat e_k)最小 Kk,使得X^k→Xk,即使得Var(e^k)最小
e
k
=
X
k
−
X
^
k
e_k=X_k - \hat X_k
ek=Xk−X^k
P
(
e
k
)
P(e_k)
P(ek) ~ N(0, P)
P 为误差协方差矩阵,
P
=
E
(
e
e
T
)
P=E(ee^T)
P=E(eeT)
t
r
(
P
)
最小时,方差最小
tr(P)最小时,方差最小
tr(P)最小时,方差最小
P = E ( e e T ) = E [ ( X k − X ^ k ) ( X k − X ^ k ) T ] P=E(ee^T)=E[(X_k-\hat X_k)(X_k-\hat X_k)^T] P=E(eeT)=E[(Xk−X^k)(Xk−X^k)T]
X
k
−
X
^
k
=
X
k
−
[
X
^
k
−
+
K
k
(
Z
k
−
H
X
^
k
−
)
]
=
X
k
−
X
^
k
−
−
K
k
Z
k
+
K
k
H
X
^
k
−
=
X
k
−
X
^
k
−
−
K
k
(
H
X
k
+
V
k
)
+
K
k
H
X
^
k
−
=
(
X
k
−
X
^
k
−
)
−
K
k
H
(
X
k
−
X
^
k
−
)
−
K
k
V
k
=
(
I
−
K
k
H
)
(
X
k
−
X
^
k
−
)
−
K
k
V
k
=
(
I
−
K
k
H
)
e
k
−
−
K
k
V
k
X_k-\hat X_k=X_k - [\hat X_k^- + K_k(Z_k-H\hat X_k^-)]\\ =X_k - \hat X_k^- - K_kZ_k + K_kH\hat X_k^-\\ =X_k - \hat X_k^- - K_k(HX_k+V_k) + K_kH\hat X_k^-\\ =(X_k-\hat X_k^-) - K_kH(X_k-\hat X_k^-)-K_kV_k\\ =(I-K_kH)(X_k-\hat X_k^-)-K_kV_k\\ =(I-K^kH)e_k^- -K_kV_k
Xk−X^k=Xk−[X^k−+Kk(Zk−HX^k−)]=Xk−X^k−−KkZk+KkHX^k−=Xk−X^k−−Kk(HXk+Vk)+KkHX^k−=(Xk−X^k−)−KkH(Xk−X^k−)−KkVk=(I−KkH)(Xk−X^k−)−KkVk=(I−KkH)ek−−KkVk
其中
e
k
−
称为先验误差
e_k^-称为先验误差
ek−称为先验误差
E [ ( I − K k H ) e k − − K k V k ] [ ( I − K k H ) e k − − K k V k ] T = E [ ( I − K k H ) e k − − K k V k ] [ e k − T ( I − K k H ) T − V k T K k T ] = E [ ( I − K k H ) e k − e k − T ( I − K k H ) T − ( I − K k H ) e k − V k T K k T − K k V k e k − T ( I − K k H ) T + K k V k V k T K k T ] E[(I-K_kH)e_k^--K_kV_k][(I-K_kH)e_k^--K_kV_k]^T\\ =E[(I-K_kH)e_k^--K_kV_k][e_k^{-T}(I-K_kH)^T-V_k^TK_k^T]\\ =E[(I-K_kH)e_k^-e_k^{-T}(I-K_kH)^T-(I-K_kH)e_k^-V_k^TK_k^T-K_kV_ke_k^{-T}(I-K_kH)^T+K_kV_kV_k^TK_k^T] E[(I−KkH)ek−−KkVk][(I−KkH)ek−−KkVk]T=E[(I−KkH)ek−−KkVk][ek−T(I−KkH)T−VkTKkT]=E[(I−KkH)ek−ek−T(I−KkH)T−(I−KkH)ek−VkTKkT−KkVkek−T(I−KkH)T+KkVkVkTKkT]
E [ ( I − K k H ) e k − V k T K k T ] = ( I − K k H ) E ( e k − V k T ) K k T = ( I − K k H ) E ( e k − ) E ( V k T ) K k T = 0 E[(I-K_kH)e_k^-V_k^TK_k^T]\\ =(I-K_kH)E(e_k^-V_k^T)K_k^T\\ =(I-K_kH)E(e_k^-)E(V_k^T)K_k^T=0 E[(I−KkH)ek−VkTKkT]=(I−KkH)E(ek−VkT)KkT=(I−KkH)E(ek−)E(VkT)KkT=0
故 P k = ( I − K k H ) E ( e k − e k − T ) ( I − K k H ) T + K k E ( V k V k T ) K k T = ( I − K k H ) P k − ( I − K k H ) T + K k R K k T = ( P k − − K k H P k − ) ( I − H T K k T ) + K k R K k T = P k − − P k − H T K k T − K k H P k − + K k H P k − H T K k T + K k R K k T P_k=(I-K_kH)E(e_k^-e_k^{-T})(I-K_kH)^T+K_kE(V_kV_k^T)K_k^T\\ =(I-K_kH)P_k^-(I-K_kH)^T+K_kRK_k^T\\ =(P_k^--K_kHP_k^-)(I-H^TK_k^T)+K_kRK_k^T\\ = P_k^--P_k^-H^TK_k^T-K_kHP_k^-+K_kHP_k^-H^TK_k^T+K_kRK_k^T Pk=(I−KkH)E(ek−ek−T)(I−KkH)T+KkE(VkVkT)KkT=(I−KkH)Pk−(I−KkH)T+KkRKkT=(Pk−−KkHPk−)(I−HTKkT)+KkRKkT=Pk−−Pk−HTKkT−KkHPk−+KkHPk−HTKkT+KkRKkT
t r ( P k ) = t r ( P k − ) − 2 t r ( K k H P k − ) + t r ( K k H P k − H T K k T ) + t r ( K k R K k T ) tr(P_k)=tr(P_k^-)-2tr(K_kHP_k^-)+tr(K_kHP_k^-H^TK_k^T)+tr(K_kRK_k^T) tr(Pk)=tr(Pk−)−2tr(KkHPk−)+tr(KkHPk−HTKkT)+tr(KkRKkT)
矩阵求导公式
d
t
r
(
A
B
)
d
A
=
B
T
\frac{dtr(AB)}{dA}=B^T
dAdtr(AB)=BT
d
t
r
(
A
B
A
T
)
d
A
=
2
A
B
\frac{dtr(ABA^T)}{dA}=2AB
dAdtr(ABAT)=2AB
令 d t r ( P k ) d K k = 0 − 2 ( H P k − ) T + 2 K k H P k − H T + 2 K k R = − P k − H T + K k ( H P k − H T + R ) = 0 \frac{dtr(P_k)}{dK_k}=0-2(HP_k^-)^T+2K_kHP_k^-H^T+2K_kR\\ =-P_k^-H^T+K_k(HP_k^-H^T+R)=0 dKkdtr(Pk)=0−2(HPk−)T+2KkHPk−HT+2KkR=−Pk−HT+Kk(HPk−HT+R)=0
则
K
k
=
P
k
−
H
T
H
P
k
−
H
T
+
R
K_k=\frac{P_k^-H^T}{HP_k^-H^T+R}
Kk=HPk−HT+RPk−HT
R
R
R 为测量噪声协方差矩阵
P
k
−
P_k^-
Pk− 为先验误差协方差矩阵
Prior / Posterior Error Covariance Matrix
X
k
=
A
X
k
−
1
+
B
U
k
−
1
+
W
k
−
1
,
W
X_k = AX_{k-1}+BU_{k-1}+W_{k-1},W
Xk=AXk−1+BUk−1+Wk−1,W~
P
(
0
,
Q
)
P(0, Q)
P(0,Q)
Z
k
=
H
W
k
+
V
k
,
V
Z_k = HW_k+V_k,V
Zk=HWk+Vk,V~
P
(
0
,
R
)
P(0, R)
P(0,R)
先验估计
X
^
k
−
=
A
X
^
k
−
1
+
B
U
k
−
1
\hat X_k^-=A\hat X_{k-1}+BU_{k-1}
X^k−=AX^k−1+BUk−1
后验估计
X
^
k
=
X
^
k
−
+
K
k
(
Z
k
−
H
X
^
k
−
)
\hat X_k=\hat X_k^-+K_k(Z_k-H\hat X_k^-)
X^k=X^k−+Kk(Zk−HX^k−)
卡尔曼增益
K
k
=
P
k
−
H
T
H
P
k
−
H
T
+
R
K_k=\frac{P_k^-H^T}{HP_k^-H^T+R}
Kk=HPk−HT+RPk−HT
求 P k − P_k^- Pk−
误差
e
k
=
X
k
−
X
^
k
e_k=X_k-\hat X_k
ek=Xk−X^k
先验误差
e
k
−
=
X
k
−
X
^
k
−
=
A
X
k
−
1
+
B
U
k
−
1
+
W
k
−
1
−
A
X
^
k
−
1
−
B
U
k
−
1
=
A
(
X
k
−
1
−
X
^
k
−
1
)
+
W
k
−
1
=
A
e
k
−
1
+
W
k
−
1
e_k^-=X_k-\hat X_k^-=AX_{k-1}+BU_{k-1}+W_{k-1}-A\hat X_{k-1}-BU_{k-1}\\ =A(X_{k-1}-\hat X_{k-1})+W_{k-1}=Ae_{k-1}+W_{k-1}
ek−=Xk−X^k−=AXk−1+BUk−1+Wk−1−AX^k−1−BUk−1=A(Xk−1−X^k−1)+Wk−1=Aek−1+Wk−1
P k − = E [ e k − e k − T ] = E [ ( A e k − 1 + W k − 1 ) ( e k − 1 T A T + W k − 1 T ) ] = E [ A e k − 1 e k − 1 T A T + A e k − 1 W k − 1 T + W k − 1 e k − 1 T A T + W k − 1 W k − 1 T ] = A E ( e k − 1 e k − 1 T ) A T + E ( W k − 1 W k − 1 T ) = A P k − 1 A T + Q P_k^-=E[e_k^-e_k^{-T}]\\ =E[(Ae_{k-1}+W_{k-1})(e^T_{k-1}A^T+W_{k-1}^T)]\\ =E[Ae_{k-1}e_{k-1}^TA^T+Ae_{k-1}W_{k-1}^T+W_{k-1}e_{k-1}^TA^T+W_{k-1}W_{k-1}^T]\\ =AE(e_{k-1}e_{k-1}^T)A^T+E(W_{k-1}W_{k-1}^T)\\ =AP_{k-1}A^T+Q Pk−=E[ek−ek−T]=E[(Aek−1+Wk−1)(ek−1TAT+Wk−1T)]=E[Aek−1ek−1TAT+Aek−1Wk−1T+Wk−1ek−1TAT+Wk−1Wk−1T]=AE(ek−1ek−1T)AT+E(Wk−1Wk−1T)=APk−1AT+Q
使用卡尔曼滤波器 估计 状态变量的值(最终五个式子)
-
预测
– 先验: X ^ k − = A X ^ k − 1 + B U k − 1 \hat X_k^-=A\hat X_{k-1}+BU_{k-1} X^k−=AX^k−1+BUk−1
– 先验误差协方差矩阵: P k − = A P k − 1 A T + Q P_k^-=AP_{k-1}A^T+Q Pk−=APk−1AT+Q -
校正
– 卡尔曼增益: K k = P k − H T H P k − H T + R K_k=\frac{P_k^-H^T}{HP_k^-H^T+R} Kk=HPk−HT+RPk−HT
– 后验估计: X ^ k = X ^ k − + K k ( Z k − H X ^ k − ) \hat X_k=\hat X_k^-+K_k(Z_k-H\hat X_k^-) X^k=X^k−+Kk(Zk−HX^k−)
– 更新误差协方差
P k = P k − − P k − H T K k T − K k H P k − + K k H P k − H T K k T + K k R K k T = P k − − P k − H T K k T − K k H P k − + K k ( H P k − H T + R ) K k T = P k − − P k − H T K k T − K k H P k − + P k − H T K k T = P k − − K k H P k − = ( I − K k H ) P k − P_k=P_k^--P_k^-H^TK_k^T-K_kHP_k^-+K_kHP_k^-H^TK_k^T+K_kRK_k^T\\ =P_k^--P_k^-H^TK_k^T-K_kHP_k^-+K_k(HP_k^-H^T+R)K_k^T\\ =P_k^--P_k^-H^TK_k^T-K_kHP_k^-+P_k^-H^TK_k^T\\ =P_k^--K_kHP_k^-\\ =(I-K_kH)P_k^- Pk=Pk−−Pk−HTKkT−KkHPk−+KkHPk−HTKkT+KkRKkT=Pk−−Pk−HTKkT−KkHPk−+Kk(HPk−HT+R)KkT=Pk−−Pk−HTKkT−KkHPk−+Pk−HTKkT=Pk−−KkHPk−=(I−KkH)Pk−
An 2D example
excel 矩阵处理函数
矩阵相乘 mmul()
转换 transpose()
取逆 minverse()
全选F2
Ctrl+Shift+Enter
该案例需要赋初始值变量
状态变量初始值 x 1 , 0 , x 2 , 0 后验估计值初始值 x ^ 1 , 0 , x ^ 2 , 0 误差协方差矩阵 P 0 状态变量初始值 x_{1,0}, x_{2,0}\\ 后验估计值初始值 \hat x_{1,0}, \hat x_{2,0}\\ 误差协方差矩阵 P_0 状态变量初始值x1,0,x2,0后验估计值初始值x^1,0,x^2,0误差协方差矩阵P0
Extended Kalman Filter(EKF)
理解
卡尔曼增益K:负责融合估计值与测量值,谁方差小,就更相信谁一些。
状态估计协方差矩阵P:初始状态与过程噪声有关,并且由于每次K的迭代,状态估计协方差矩阵也在迭代(因为使用了上一次的结果作为下一次的初始状态,上一次的结果来自于卡尔曼增益K对观测与估计的融合,状态估计协方差会减小)
过程噪声协方差矩阵Q:来自于世界中的不确定性,这个值怎么选,我也不太清楚,不知道如何度量世界中的不确定性。
测量误差协方差矩阵R:来自于传感器误差(我猜是可以通过试验获得,比如测量100次,然后记录数据)
其余的就是要给一个目标的初始状态,然后根据观测,不断地更新估计,得到一个稳定的K。