文章目录
图解部分来自 知乎问题第一个回答的视频部分（感谢作者的翻译）：https://www.zhihu.com/question/23971601；
推导部分转自作者白巧克力亦唯心https://heyijia.blog.csdn.net/article/details/17487467。
- 卡尔曼滤波一般用于优化估算一些无法直接测量但可以间接测量的量，或用于从受误差影响的传感器测量值中估算出系统状态。
- 系统的状态可以由状态转移预测得到预测值，及传感器测量得到测量值，但其中都包含噪声，因此需要把这两个值结合得到估计值
1.图解卡尔曼滤波
-
假设有个线性状态转移系统，系统真实的状态可以表示为
x k = A x k − 1 + B u k + ω k y k = C x k + v k \begin{aligned} x_k &= Ax_{k-1} + Bu_k + \omega_k \\ y_k &= Cx_k + v_k \end{aligned} xkyk=Axk−1+Buk+ωk=Cxk+vk
其中- x k x_k xk表示为 k k k时刻系统状态，
- u k u_k uk表示为 k k k时刻系统输入，
- ω k \omega_k ωk表示为 k k k时刻系统噪声
- y k y_k yk表示为 k k k时刻传感器的测量值
- v k v_k vk表示为 k k k时刻传感器测量噪声
-
当估计系统状态及估计传感器输出值时
x ^ k ′ = A x ^ k − 1 + B u k y ^ k = C x ^ k ′ \begin{aligned} \hat{x}_k^\prime &= A\hat{x}_{k-1} + Bu_k \\ \hat{y}_k &= C\hat{x}_k^\prime \end{aligned} x^k′y^k=Ax^k−1+Buk=Cx^k′
其中- x ^ k − 1 \hat{x}_{k-1} x^k−1 表示为 k − 1 k-1 k−1时刻的估计值（结合预测值和测量值的结果）
- x ^ k ′ \hat{x}_k^\prime x^k′表示 k k k时刻的预测值
- y ^ k \hat{y}_k y^k表示 k k k时刻的预测值 x ^ k ′ \hat{x}_k^\prime x^k′得到的预测测量值
-
使用权重结合预测值和真实的测量值，得到估计值
x ^ k = x ^ k ′ + K k ( y k − y ^ k ) = x ^ k ′ + K k ( y k − C x ^ k ′ ) = ( 1 − K k C ) x ^ k ′ + K k y k \begin{aligned} \hat{x}_k &= \hat{x}_k^\prime + K_k(y_k -\hat{y}_k) \\ &= \hat{x}_k^\prime + K_k(y_k -C\hat{x}_k^\prime) \\ &= (1-K_kC)\hat{x}_k^\prime + K_ky_k \end{aligned} x^k=x^k′+Kk(yk−y^k)=x^k′+Kk(yk−Cx^k′)=(1−KkC)x^k′+Kkyk
-
假设有初始位置 x ^ k − 1 \hat{x}_{k-1} x^k−1，代表小车的上一个位置，并且概率分布方差为 σ k − 1 \sigma_{k-1} σk−1
-
根据 x ^ k ′ = A x ^ k − 1 + B u k \hat{x}_k^\prime = A\hat{x}_{k-1} + Bu_k x^k′=Ax^k−1+Buk得到k时刻的预测位置，但此时由于误差 w k w_k wk及系数 A A A的存在，概率分布的方差 σ k ′ \sigma_k^\prime σk′相比 σ k − 1 \sigma_{k-1} σk−1变得更大
-
若有一个传感器测量值为 y k y_k yk，同时有自己的概率分布方差为 R R R
-
最终想要的估计值 x ^ k \hat{x}_k x^k就是通过权重结合 x ^ k ′ , y k \hat{x}_k^\prime,y_k x^k′,yk，使结合后的概率分布方差 σ k \sigma_k σk最小，并且小于上次的估计方差 σ k − 1 \sigma_{k-1} σk−1。
从图解原理反过来分析看卡尔曼滤波算法计算过程
- 预测数值 x ^ ′ \hat{x}^\prime x^′概率分布的方差 σ k ′ \sigma_k^\prime σk′由 x ^ k − 1 \hat{x}_{k-1} x^k−1的方差计算得到，有参数A的传递，及叠加噪声 ω \omega ω的方差Q
- 参数 K k K_k Kk的计算中需要有 x ^ ′ \hat{x}^\prime x^′的方差 σ k ′ \sigma_k^\prime σk′及 y k y_k yk方差R的参与
- 在 x ^ k \hat{x}_k x^k概率分布方差 σ k \sigma_k σk迭代计算中，总会有噪声 w k , v k w_k,v_k wk,vk分布方差的参与。
2.公式推导
以下公式中使用 Z Z Z代替上面图解中的 y y y，代表传感器测量值， H H H代替 C C C。
2.1.概述
- 卡尔曼滤波器是在估计线性系统状态的过程中，以最小均方误差为目的而推导出的几个递推数学等式,也可以从贝叶斯推断的角度来推导。
- 卡尔曼滤波利用预测+测量反馈估计当前系统状态
2.2.数学知识
2.2.1均方误差MSE
样本量一定时，评价一个点估计的好坏的指标
M
S
E
(
θ
^
)
=
E
(
θ
^
−
θ
)
2
=
∫
(
θ
^
−
θ
)
2
p
(
x
)
d
x
=
E
[
(
θ
^
−
E
(
θ
^
)
)
+
(
E
(
θ
^
)
−
θ
)
]
2
=
E
(
θ
^
−
E
(
θ
^
)
)
2
+
E
(
E
(
θ
^
)
−
θ
)
2
+
2
E
[
(
θ
^
−
E
(
θ
^
)
)
(
E
(
θ
^
)
−
θ
)
]
=
D
(
θ
^
)
+
E
[
θ
2
−
2
θ
^
θ
−
E
2
(
θ
^
)
+
2
θ
^
E
(
θ
^
)
]
=
D
(
θ
^
)
+
E
(
θ
2
)
−
2
E
(
θ
^
θ
)
+
E
2
(
θ
^
)
=
D
(
θ
^
)
+
[
E
(
θ
^
)
−
θ
]
2
\begin{aligned} MSE(\hat{\theta}) &= E(\hat{\theta} - \theta)^2 = \int (\hat{\theta} - \theta)^2 p(x)dx\\ &= E[(\hat{\theta}-E(\hat{\theta})) + (E(\hat{\theta}) - \theta)]^2 \\ &= E(\hat{\theta}-E(\hat{\theta}))^2 + E(E(\hat{\theta}) - \theta)^2 + 2E[(\hat{\theta}-E(\hat{\theta}))(E(\hat{\theta}) - \theta)] \\ &=D(\hat{\theta}) + E[\theta^2-2\hat{\theta}\theta - E^2(\hat{\theta}) + 2\hat{\theta}E(\hat{\theta})] \\ & = D(\hat{\theta}) + E(\theta^2) -2E(\hat{\theta}\theta)+ E^2(\hat{\theta}) \\ &= D(\hat{\theta}) + [E(\hat{\theta}) -\theta]^2 \end{aligned}
MSE(θ^)=E(θ^−θ)2=∫(θ^−θ)2p(x)dx=E[(θ^−E(θ^))+(E(θ^)−θ)]2=E(θ^−E(θ^))2+E(E(θ^)−θ)2+2E[(θ^−E(θ^))(E(θ^)−θ)]=D(θ^)+E[θ2−2θ^θ−E2(θ^)+2θ^E(θ^)]=D(θ^)+E(θ2)−2E(θ^θ)+E2(θ^)=D(θ^)+[E(θ^)−θ]2
均方误差为点估计的方差和偏差的平方组成，如果点估计是无偏估计，均方误差和方差是一致的
2.2.2 方差
D ( x ) = E ( x − E ( x ) ) 2 D(x) = E(x-E(x))^2 D(x)=E(x−E(x))2
2.2.3协方差
c
o
v
(
X
,
Y
)
=
E
(
(
x
−
E
(
x
)
)
(
y
−
E
(
y
)
)
)
cov(X,Y) = E((x-E(x))(y-E(y)))
cov(X,Y)=E((x−E(x))(y−E(y)))
对于多元高斯分布的方差
∑
=
[
D
X
1
c
o
v
(
X
1
,
X
2
)
…
c
o
v
(
X
1
,
X
n
)
c
o
v
(
X
1
,
X
2
)
D
X
2
⋯
c
o
v
(
X
2
,
X
n
)
⋮
⋮
⋱
⋮
c
o
v
(
X
1
,
X
n
)
c
o
v
(
X
1
,
X
2
)
…
D
X
n
]
\sum = \begin{bmatrix}DX_1 & cov(X_1,X_2) & \dots&cov(X_1,X_n) \\ cov(X_1,X_2) &DX_2 &\cdots &cov(X_2,X_n)\\ \vdots& \vdots &\ddots &\vdots \\ cov(X_1,X_n) & cov(X_1,X_2) &\dots&DX_n \end{bmatrix}
∑=⎣⎢⎢⎢⎡DX1cov(X1,X2)⋮cov(X1,Xn)cov(X1,X2)DX2⋮cov(X1,X2)…⋯⋱…cov(X1,Xn)cov(X2,Xn)⋮DXn⎦⎥⎥⎥⎤
2.3.线性系统
2.3.1线性系统的状态差分方程
x k = A x k − 1 + B u k − 1 + w k − 1 x_k = Ax_{k-1} + Bu_{k-1} + w_{k-1} xk=Axk−1+Buk−1+wk−1
- x是系统的真实状态向量， n × 1 n\times1 n×1维
- u是系统的输入, k × 1 k\times1 k×1
- w是系统噪声，随机变量， n × 1 n\times1 n×1，实际会由噪声造成误差
- A是状态转移矩阵, n × n n\times n n×n
- B是将输入转为状态的矩阵, n × k n\times k n×k
由于实际计算中一旦产生误差，由于状态递推公式，会不断的由A放大误差。因此不能直接使用状态转移矩阵计算当前状态。此时计算的结果相当于先验结果。
举例：匀加速小车，受到合力
f
t
f_t
ft
x
t
=
x
t
−
1
+
(
x
t
−
1
⋅
∗
Δ
t
)
+
f
t
2
m
(
Δ
t
)
2
x
t
˙
=
x
t
−
1
˙
+
f
t
m
Δ
t
\begin{aligned} x_t &= x_{t-1} + (x_{t-1}^{\cdot} * \Delta t) + \frac{f_t}{2m}(\Delta t)^2 \\ \dot{x_t} &= \dot{x_{t-1}} + \frac{f_t}{m}\Delta t \end{aligned}
xtxt˙=xt−1+(xt−1⋅∗Δt)+2mft(Δt)2=xt−1˙+mftΔt
那么系统状态方程为
[
x
t
x
t
˙
]
=
[
1
Δ
t
0
1
]
[
x
t
−
1
x
t
−
1
˙
]
+
[
Δ
t
2
2
Δ
t
]
f
t
m
\begin{aligned} \begin{bmatrix}x_t \\ \dot{x_t} \end{bmatrix} = \begin{bmatrix}1 & \Delta t\\0& 1 \end{bmatrix} \begin{bmatrix}x_{t-1}\\ \dot{x_{t-1}} \end{bmatrix} + \begin{bmatrix}\frac{\Delta t^2}{2}\\ \Delta t \end{bmatrix} \frac{f_t}{m} \end{aligned}
[xtxt˙]=[10Δt1][xt−1xt−1˙]+[2Δt2Δt]mft
2.3.2 测量值
由传感器测量的测量结果相当于后验
z
k
=
H
x
k
+
v
k
z_k = Hx_k + v_k
zk=Hxk+vk
- z是真实测量值 m × 1 m \times 1 m×1
- v是测量噪声
- H是状态变量到测量的转换矩阵
测量值有很大的噪声，不能直接用于计算。
举例：对于匀加速模型，使用超声波测量小车距离人的距离，那么方程为
z
k
=
[
1
0
]
[
x
t
x
t
˙
]
z_k = \begin{bmatrix}1 & 0 \end{bmatrix} \begin{bmatrix}x_t \\ \dot{x_t} \end{bmatrix}
zk=[10][xtxt˙]
2.3.3系统噪声w,测量噪声v
假设服从如下多元高斯分布，并且w,v是相互独立的。其中
Q
,
R
Q,R
Q,R为噪声变量的协方差矩阵
p
(
w
)
∼
N
(
0
,
Q
)
p
(
v
)
∼
N
(
0
,
R
)
\begin{aligned} p(w) \sim N(0,Q) \\ p(v) \sim N(0,R) \end{aligned}
p(w)∼N(0,Q)p(v)∼N(0,R)
例如：匀加速小车，假设系统噪声只存在速度上的分量，且速度
x
˙
\dot{x}
x˙噪声的方差是一个常量0.01；位移
x
x
x系统噪声为0。那么测量噪声的协方差矩阵
Q
=
[
0
0
0
0.01
]
Q = \begin{bmatrix}0 & 0 \\0& 0.01 \end{bmatrix}
Q=[0000.01]
测量数据只有位移
x
x
x，协方差矩阵R
1
×
1
1\times 1
1×1，即测量噪声本身方差
2.3.4 最优估计:结合前验（状态转移方程计算数据）和后验(测量值)
- x ^ k \hat{x}_k x^k是估计值
- x ^ k ′ \hat{x}_k^\prime x^k′是先验值，是预测值，即状态转移方程计算数据
- z ^ k \hat{z}_k z^k是对测量值的预测，即使用 z ^ k = H x ^ k ′ \hat{z}_k = H\hat{x}_k^\prime z^k=Hx^k′，即 x ^ k ′ \hat{x}_k^\prime x^k′预测的测量值
- z k z_k zk是实际的测量值,满足 z k = H x k + v k z_k = Hx_k + v_k zk=Hxk+vk,其中 x k x_k xk是真实的状态
根据加权方法得到估计值
x
^
k
=
x
^
k
′
+
K
k
(
z
k
−
z
^
k
)
\begin{aligned} \hat{x}_k &=\hat{x}_k^\prime + K_k(z_k - \hat{z}_k) \end{aligned}
x^k=x^k′+Kk(zk−z^k)
- 其中k是指第k次的计算， K K K是指对测量值得加权，每次迭代 K K K数值都不同，需要根据最小MSE原则求出最优的 K k K_k Kk，从而求出估计值 x ^ \hat{x} x^.
- z k − z ^ k z_k - \hat{z}_k zk−z^k称为残差：预测的测量值和实际测量值间的差距。
- 若 z k − z ^ k = 0 z_k - \hat{z}_k = 0 zk−z^k=0，预测的测量值和实际测量值相同
2.3.5预测值和真实值的协方差矩阵，估计值和真实值的协方差矩阵
-
协方差矩阵
令 P k P_k Pk为协方差矩阵(实际数值 x k x_k xk与估计值 x ^ k \hat{x}_k x^k平方的期望）
P k = E ( e k e k T ) = E [ ( x k − x ^ k ) ( x k − x ^ k ) T ] P_k = E(e_ke_k^T) = E[(x_k-\hat{x}_k)(x_k-\hat{x}_k)^T] Pk=E(ekekT)=E[(xk−x^k)(xk−x^k)T]
其中 x k x_k xk是 n × 1 n \times 1 n×1维向量，而 P k P_k Pk是 n × n n\times n n×n维向量。并且
( x k − x ^ k ) ( x k − x ^ k ) T = [ x k 1 − x ^ k 1 x k 2 − x ^ k 2 ⋯ x k n − x ^ k n ] [ x k 1 − x ^ k 1 x k 2 − x ^ k 2 ⋯ x k n − x ^ k n ] = [ ( x k 1 − x ^ k 1 ) 2 ( x k 1 − x ^ k 1 ) ( x k 2 − x ^ k 2 ) ⋯ ( x k 1 − x ^ k 1 ) ( x k n − x ^ k n ) ( x k 1 − x ^ k 1 ) ( x k 2 − x ^ k 2 ) ( x k 2 − x ^ k 2 ) 2 ⋯ ( x k 2 − x ^ k 2 ) ( x k n − x ^ k n ) ⋮ ⋮ ⋱ ⋮ ( x k 1 − x ^ k 1 ) ( x n − x ^ k n ) ( x k 2 − x ^ k 2 ) ( x n − x ^ k n ) ⋯ ( x k n − x ^ k n ) 2 ] \begin{aligned} (x_k-\hat{x}_k)(x_k-\hat{x}_k)^T&= \begin{bmatrix} x^1_k-\hat{x}_k^1 \\ x_k^2-\hat{x}_k^2 \\ \cdots \\ x_k^n-\hat{x}_k^n \end{bmatrix} \begin{bmatrix} x_k^1-\hat{x}_k^1 & x_k^2-\hat{x}_k^2 & \cdots & x_k^n-\hat{x}_k^n \end{bmatrix} \\ &=\begin{bmatrix} (x_k^1-\hat{x}_k^1)^2 &(x_k^1-\hat{x}_k^1)(x_k^2-\hat{x}_k^2)&\cdots&(x_k^1-\hat{x}_k^1)( x_k^n-\hat{x}_k^n) \\ (x_k^1-\hat{x}_k^1)(x_k^2-\hat{x}_k^2) & (x_k^2-\hat{x}_k^2)^2 & \cdots &(x_k^2-\hat{x}_k^2)( x_k^n-\hat{x}_k^n) \\ \vdots & \vdots & \ddots & \vdots \\ (x_k^1-\hat{x}_k^1)( x_n-\hat{x}_k^n) & (x_k^2-\hat{x}_k^2)( x_n-\hat{x}_k^n) & \cdots & ( x_k^n-\hat{x}_k^n)^2 \end{bmatrix} \\ \end{aligned} (xk−x^k)(xk−x^k)T=⎣⎢⎢⎡xk1−x^k1xk2−x^k2⋯xkn−x^kn⎦⎥⎥⎤[xk1−x^k1xk2−x^k2⋯xkn−x^kn]=⎣⎢⎢⎢⎡(xk1−x^k1)2(xk1−x^k1)(xk2−x^k2)⋮(xk1−x^k1)(xn−x^kn)(xk1−x^k1)(xk2−x^k2)(xk2−x^k2)2⋮(xk2−x^k2)(xn−x^kn)⋯⋯⋱⋯(xk1−x^k1)(xkn−x^kn)(xk2−x^k2)(xkn−x^kn)⋮(xkn−x^kn)2⎦⎥⎥⎥⎤
注意以下协方差矩阵对角线的元素就是对每个状态分量的方差
E [ ( x k − x ^ k ) ( x k − x ^ k ) T ] = [ E ( x e r r 1 x e r r 1 ) E ( x e r r 1 x e r r 2 ) ⋯ E ( x e r r 1 x e r r n ) E ( x e r r 2 x e r r 1 ) E ( x e r r 2 x e r r 2 ) ⋯ E ( x e r r 2 x e r r n ) ⋮ ⋮ ⋱ ⋮ E ( x e r r n x e r r 1 ) E ( x e r r n x e r r 2 ) ⋯ E ( x e r r n x e r r n ) ] \begin{aligned} E[(x_k-\hat{x}_k)(x_k-\hat{x}_k)^T] & = \begin{bmatrix} E(x_{err}^1x_{err}^1) & E(x_{err}^1x_{err}^2) & \cdots & E(x_{err}^1x_{err}^n) \\ E(x_{err}^2x_{err}^1) & E(x_{err}^2x_{err}^2) & \cdots & E(x_{err}^2x_{err}^n) \\ \vdots & \vdots & \ddots & \vdots \\ E(x_{err}^nx_{err}^1) & E(x_{err}^nx_{err}^2) & \cdots & E(x_{err}^nx_{err}^n) \end{bmatrix} \end{aligned} E[(xk−x^k)(xk−x^k)T]=⎣⎢⎢⎢⎡E(xerr1xerr1)E(xerr2xerr1)⋮E(xerrnxerr1)E(xerr1xerr2)E(xerr2xerr2)⋮E(xerrnxerr2)⋯⋯⋱⋯E(xerr1xerrn)E(xerr2xerrn)⋮E(xerrnxerrn)⎦⎥⎥⎥⎤
举例对于匀加速agv, x = { s , v } x =\{s,v\} x={s,v}
P k = E ( e k e k T ) = E [ ( x k − x ^ k ) ( x k − x ^ k ) T ] = [ E ( s e r r s e r r T ) E ( s e r r v e r r T ) E ( v e r r s e r r T ) E ( v e r r v e r r T ) ] \begin{aligned} P_k = E(e_ke_k^T) &= E[(x_k-\hat{x}_k)(x_k-\hat{x}_k)^T] \\ &=\begin{bmatrix} E(s_{err}s_{err}^T) & E(s_{err}v_{err}^T) \\ E(v_{err}s_{err}^T) & E(v_{err}v_{err}^T) \end{bmatrix} \end{aligned} Pk=E(ekekT)=E[(xk−x^k)(xk−x^k)T]=[E(serrserrT)E(verrserrT)E(serrverrT)E(verrverrT)] -
预测值与真实值之间的协方差矩阵
P k ′ = E ( e k ′ e k ′ T ) = E [ ( x k − x ^ k ′ ) ( x k − x ^ k ′ ) T ] \begin{aligned} P_k^\prime = E(e_k^\prime {e^\prime_k}^T) &= E[(x_k-\hat{x}_k^\prime)(x_k-\hat{x}_k^\prime)^T] \end{aligned} Pk′=E(ek′ek′T)=E[(xk−x^k′)(xk−x^k′)T] -
估计值与真实值之间的协方差矩阵
- 由于
x k = A x k − 1 + B u k − 1 + w k − 1 z k = H x k + v k x ^ k = x ^ k ′ + K k ( z k − z ^ k ) z ^ k = H x ^ k ′ P k = E ( e k e k T ) = E [ ( x k − x ^ k ) ( x k − x ^ k ) T ] \begin{aligned} x_k &= Ax_{k-1} + Bu_{k-1} + w_{k-1} \\ z_k &= Hx_k + v_k \\ \hat{x}_k &=\hat{x}_k^\prime + K_k(z_k - \hat{z}_k)\\ \hat{z}_k &= H\hat{x}_k^\prime \\ P_k &= E(e_ke_k^T) = E[(x_k-\hat{x}_k)(x_k-\hat{x}_k)^T] \end{aligned} xkzkx^kz^kPk=Axk−1+Buk−1+wk−1=Hxk+vk=x^k′+Kk(zk−z^k)=Hx^k′=E(ekekT)=E[(xk−x^k)(xk−x^k)T]
实际情况中，总要有噪声( w k − 1 , v k w_{k-1}, v_k wk−1,vk)，但预测时不会考虑噪声。
x ^ k = x ^ k ′ + K k ( z k − z ^ k ) = x ^ k ′ + K k ( H x k + v x − H x ^ k ′ ) = ( I − K k H ) x ^ k ′ + K k H x k + K k v k \begin{aligned} \hat{x}_k &=\hat{x}_k^\prime + K_k(z_k - \hat{z}_k) \\ &= \hat{x}_k^\prime + K_k(Hx_k + v_x - H\hat{x}_k^\prime)\\ &=(I - K_kH)\hat{x}_k^\prime + K_kHx_k +K_kv_k\\ \end{aligned} x^k=x^k′+Kk(zk−z^k)=x^k′+Kk(Hxk+vx−Hx^k′)=(I−KkH)x^k′+KkHxk+Kkvk - 并有
( A − B ) ( A − B ) T = A A T + B B T − A B T − B A T (A-B)(A-B)^T = AA^T + BB^T - AB^T -BA^T (A−B)(A−B)T=AAT+BBT−ABT−BAT - 推导
P k = E ( e k e k T ) = E [ ( x k − x ^ k ) ( x k − x ^ k ) T ] = E [ { ( I − K k H ) ( x k − x ^ k ′ ) − K k v k } { ( I − K k H ) ( x k − x ^ k ′ ) − K k v k } T ] = ( I − K k H ) E [ ( x k − x ^ k ′ ) ( x k − x ^ k ′ ) T ] ( I − K k H ) T + K k E ( v k v k T ) K K T − E [ ( I − K k H ) ( x k − x ^ k ′ ) v k T K k T ] − E [ K k v k ( x k − x ^ k ′ ) T ( I − K k H ) T ] \begin{aligned} P_k = E(e_ke_k^T) &= E[(x_k-\hat{x}_k)(x_k-\hat{x}_k)^T] \\ & = E[\{(I - K_kH)(x_k - \hat{x}_k^\prime) -K_kv_k\}\{(I - K_kH)(x_k - \hat{x}_k^\prime) -K_kv_k\}^T] \\ & = (I - K_kH) E[(x_k - \hat{x}_k^\prime)(x_k - \hat{x}_k^\prime)^T](I - K_kH)^T + K_kE(v_kv_k^T)K_K^T \\ &\quad -E[(I - K_kH)(x_k - \hat{x}_k^\prime)v_k^TK_k^T] - E[K_kv_k(x_k - \hat{x}_k^\prime)^T(I - K_kH)^T] \end{aligned} Pk=E(ekekT)=E[(xk−x^k)(xk−x^k)T]=E[{(I−KkH)(xk−x^k′)−Kkvk}{(I−KkH)(xk−x^k′)−Kkvk}T]=(I−KkH)E[(xk−x^k′)(xk−x^k′)T](I−KkH)T+KkE(vkvkT)KKT−E[(I−KkH)(xk−x^k′)vkTKkT]−E[Kkvk(xk−x^k′)T(I−KkH)T]
由于系统状态 x x x与测量噪声 v v v相互独立，因此
P k = E ( e k e k T ) = ( I − K k H ) E [ ( x k − x ^ k ′ ) ( x k − x ^ 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 ′ − 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 \begin{aligned} P_k = E(e_ke_k^T) &=(I - K_kH) E[(x_k - \hat{x}_k^\prime)(x_k - \hat{x}_k^\prime)^T](I - K_kH)^T + K_kE(v_kv_k^T)K_K^T \\ &=(I - K_kH)P_k^\prime(I - K_kH)^T + K_kRK_k^T \\ & = P_k^\prime - P_k^\prime H^TK_k^T - K_kHP_k^\prime + K_kHP_k^\prime H^TK_k^T + K_kRK_k^T \\ &=P_k^\prime - P_k^\prime H^TK_k^T - K_kHP_k^\prime + K_k(HP_k^\prime H^T + R)K_k^T \\ \end{aligned} Pk=E(ekekT)=(I−KkH)E[(xk−x^k′)(xk−x^k′)T](I−KkH)T+KkE(vkvkT)KKT=(I−KkH)Pk′(I−KkH)T+KkRKkT=Pk′−Pk′HTKkT−KkHPk′+KkHPk′HTKkT+KkRKkT=Pk′−Pk′HTKkT−KkHPk′+Kk(HPk′HT+R)KkT
- 由于
2.3.6最小化MSE
- 求MSE
协方差矩阵对角线元素就是方差，那么协方差矩阵对角线求和即MSE（求迹用T表示）
P k = 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 T ( P k ) = T ( P k ′ ) − 2 T ( K k H P k ′ ) + T ( K k ( H P k ′ H T + R ) K k T ) \begin{aligned} P_k &=P_k^\prime - P_k^\prime H^TK_k^T - K_kHP_k^\prime + K_k(HP_k^\prime H^T + R)K_k^T \\ T(P_k) &= T(P_k^\prime) - 2T(K_kHP_k^\prime) + T(K_k(HP_k^\prime H^T + R)K_k^T) \end{aligned} PkT(Pk)=Pk′−Pk′HTKkT−KkHPk′+Kk(HPk′HT+R)KkT=T(Pk′)−2T(KkHPk′)+T(Kk(HPk′HT+R)KkT) - 最小均方差MSE，即
min
T
(
P
k
)
\min T(P_k)
minT(Pk)，求导得最优
K
k
K_k
Kk
d T ( P k ) d K k = 0 − 2 ( H P k ′ ) T + K k ( H P k ′ H T + R ) T + K k ( H P k ′ H T + R ) = − 2 ( H P k ′ ) T + 2 K k ( H P k ′ H T + R ) = 0 K k = P k ′ H T ( H P k ′ H T + R ) − 1 \begin{aligned} \frac{dT(P_k)}{dK_k} &=0 -2(HP_k^\prime)^T + K_k(HP_k^\prime H^T + R)^T + K_k(HP_k^\prime H^T + R) \\ &=-2(HP_k^\prime)^T + 2K_k(HP_k^\prime H^T + R) = 0\\ K_k &= P_k^\prime H^T(HP_k^\prime H^T + R)^{-1} \end{aligned} dKkdT(Pk)Kk=0−2(HPk′)T+Kk(HPk′HT+R)T+Kk(HPk′HT+R)=−2(HPk′)T+2Kk(HPk′HT+R)=0=Pk′HT(HPk′HT+R)−1
结论：要使MSE最小，取值 K k = P k ′ H T ( H P k ′ H T + R ) − 1 K_k = P_k^\prime H^T(HP_k^\prime H^T + R)^{-1} Kk=Pk′HT(HPk′HT+R)−1。其中 H , R H,R H,R是常量（或常矢量）
举例：匀加速运动agv,H = 1,
K
k
=
P
k
′
P
k
′
+
R
=
1
1
+
R
/
P
k
′
K_k = \frac{P_k^\prime}{P_k^\prime + R} = \frac{1}{1 + R/P_k^\prime}
Kk=Pk′+RPk′=1+R/Pk′1
若
P
k
′
P_k^\prime
Pk′越大，
K
k
K_k
Kk越大，那么估计值
x
^
k
=
x
^
k
′
+
K
k
(
z
k
−
z
^
k
)
\hat{x}_k =\hat{x}_k^\prime + K_k(z_k - \hat{z}_k)
x^k=x^k′+Kk(zk−z^k)更重视测试值
z
k
z_k
zk；若
z
k
=
z
^
k
z_k = \hat{z}_k
zk=z^k，
x
^
k
=
x
^
k
′
\hat{x}_k =\hat{x}_k^\prime
x^k=x^k′。
- 由
K
k
K_k
Kk求解
P
k
P_k
Pk
P k = 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 ( H P k ′ H T + R ) − 1 ( H P k ′ H T + R ) K k T = P k ′ − K k H P k ′ = ( 1 − K k H ) P k ′ = P k ′ − P k ′ H T ( H P k ′ H T + R ) − 1 H P k ′ \begin{aligned} P_k &= P_k^\prime - P_k^\prime H^TK_k^T - K_kHP_k^\prime + K_k(HP_k^\prime H^T + R)K_k^T \\ &= P_k^\prime - P_k^\prime H^TK_k^T - K_kHP_k^\prime + P_k^\prime H^T(HP_k^\prime H^T + R)^{-1}(HP_k^\prime H^T + R)K_k^T \\ &=P_k^\prime- K_kHP_k^\prime = (1-K_kH)P_k^\prime\\ &= P_k^\prime - P_k^\prime H^T(HP_k^\prime H^T + R)^{-1}HP_k^\prime \end{aligned} Pk=Pk′−Pk′HTKkT−KkHPk′+Kk(HPk′HT+R)KkT=Pk′−Pk′HTKkT−KkHPk′+Pk′HT(HPk′HT+R)−1(HPk′HT+R)KkT=Pk′−KkHPk′=(1−KkH)Pk′=Pk′−Pk′HT(HPk′HT+R)−1HPk′ - 求解未知
P
k
′
P_k^\prime
Pk′
有递推公式
x ^ k ′ = A x ^ k − 1 + B u k − 1 P k ′ = E ( e k ′ e k ′ T ) = E [ ( x k − x ^ k ′ ) ( x k − x ^ k ′ ) T ] = E [ ( A x k − 1 + B u k − 1 + ω k − 1 − A x ^ k − 1 − B u k − 1 ) ( A x k − 1 + B u k − 1 + ω k − 1 − A x ^ k − 1 − B u k − 1 ) T ] = E [ ( A ( x k − 1 − x ^ k − 1 ) + ω k − 1 ) ( A ( x k − 1 − x ^ k − 1 ) + ω k − 1 ) T ] = A E [ ( x k − 1 − x ^ k − 1 ) ( x k − 1 − x ^ k − 1 ) T ] A T + E ( w k − 1 w k − 1 T ) = A P k − 1 A T + Q \begin{aligned} \hat{x}_k^\prime &= A\hat{x}_{k-1} + Bu_{k-1} \\ P_k^\prime &= E(e_k^\prime {e^\prime_k}^T) \\ &= E[(x_k-\hat{x}_k^\prime)(x_k-\hat{x}_k^\prime)^T] \\ & = E[(Ax_{k-1} + Bu_{k-1} + \omega_{k-1} - A\hat{x}_{k-1} - Bu_{k-1})(Ax_{k-1} + Bu_{k-1} + \omega_{k-1} - A\hat{x}_{k-1} - Bu_{k-1})^T] \\ &=E[(A(x_{k-1} - \hat{x}_{k-1}) + \omega_{k-1})(A(x_{k-1} - \hat{x}_{k-1}) + \omega_{k-1})^T] \\ &= AE[(x_{k-1} - \hat{x}_{k-1})(x_{k-1} - \hat{x}_{k-1})^T]A^T + E(w_{k-1}w_{k-1}^T)\\ &=AP_{k-1}A^T + Q \end{aligned} x^k′Pk′=Ax^k−1+Buk−1=E(ek′ek′T)=E[(xk−x^k′)(xk−x^k′)T]=E[(Axk−1+Buk−1+ωk−1−Ax^k−1−Buk−1)(Axk−1+Buk−1+ωk−1−Ax^k−1−Buk−1)T]=E[(A(xk−1−x^k−1)+ωk−1)(A(xk−1−x^k−1)+ωk−1)T]=AE[(xk−1−x^k−1)(xk−1−x^k−1)T]AT+E(wk−1wk−1T)=APk−1AT+Q
系统状态 x x x与噪声 ω \omega ω相互独立
2.3.7 递推的算法步骤
- 假设初始值 P 0 , Q , R P_0,Q,R P0,Q,R, x 0 , u o x_0,u_o x0,uo
- 递推计算
x ^ k ′ = A x ^ k − 1 + B u k − 1 P k ′ = A P k − 1 A T + Q \begin{aligned} \hat{x}_k^\prime &= A\hat{x}_{k-1} + Bu_{k-1} \\ P_k^\prime &=AP_{k-1}A^T + Q \end{aligned} x^k′Pk′=Ax^k−1+Buk−1=APk−1AT+Q - 计算参数
K
k
K_k
Kk
K k = P k ′ H T ( H P k ′ H T + R ) − 1 K_k = P_k^\prime H^T(HP_k^\prime H^T + R)^{-1} Kk=Pk′HT(HPk′HT+R)−1 - 得到传感器测试值 z k z_k zk
- 计算估计值
x ^ k = x ^ k ′ + K k ( z k − z ^ k ) \hat{x}_k =\hat{x}_k^\prime + K_k(z_k - \hat{z}_k) x^k=x^k′+Kk(zk−z^k) - 递推
P
k
P_k
Pk
P k = ( 1 − K k H ) P k ′ P_k =(1-K_kH)P_k^\prime Pk=(1−KkH)Pk′
扫一扫
1227
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
