卡尔曼滤波
卡尔曼滤波是一个递归估计算法，用于估计具有不确定性的隐藏状态。其不确定性来自于两个方面，其一是由前一时刻状态推测下一时刻状态的过程中的过程噪声，其二是观测过程中的噪声。卡尔曼滤波假设这两种噪声都服从正态分布，引入卡尔曼增益K，根据推测值 x k ^ \hat{x_k} xk^和测量值 z k z_k zk，推导出一个当前时刻的最优估计。
第一步：估计状态
x k ^ = A x k − 1 ^ + B u k − 1 ^ + w k − 1 z k ^ = H x k ^ + v k \hat{x_k} =A\hat{x_{k-1}}+B\hat{u_{k-1}}+w_{k-1}\\ \hat{z_k} =H\hat{x_k}+v_k xk^=Axk−1^+Buk−1^+wk−1zk^=Hxk^+vk
x k ^ ： k 时刻状态的估计值 z k ^ ： k 时刻状态的估计测量值 H ： 观测矩阵，将状态值统一到观测值的单位下。如果测量值和系统状态本身就是统一的，那么 H 实际上就是一个单位阵。 w k − 1 ： k − 1 时刻的过程噪音，可看作常数 p ( w k − 1 ) ∼ N ( 0 , Q ) v k ： k 时刻的观测噪音，可看作常数 p ( v k ) ∼ N ( 0 , R ) \begin{aligned} &\hat{x_k}：&&k时刻状态的估计值\\ &\hat{z_k}：&&k时刻状态的估计测量值\\ &H：&&观测矩阵，将状态值统一到观测值的单位下。如果测量值和系统状态本身就是统一的，那么H 实际上就是一个单位阵。\\ &w_{k-1}：&&k-1时刻的过程噪音，可看作常数 \quad p( w_{k-1})\sim N(0,Q)\\ &v_k：&&k时刻的观测噪音，可看作常数 \quad p(v_k)\sim N(0,R)\\ \end{aligned} xk^：zk^：H：wk−1：vk：k时刻状态的估计值k时刻状态的估计测量值观测矩阵，将状态值统一到观测值的单位下。如果测量值和系统状态本身就是统一的，那么H实际上就是一个单位阵。k−1时刻的过程噪音，可看作常数p(wk−1)∼N(0,Q)k时刻的观测噪音，可看作常数p(vk)∼N(0,R)
由
x
k
−
1
^
\hat{x_{k-1}}
xk−1^到
x
k
^
\hat{x_k}
xk^这一估计过程的不确定性用协方差
P
k
P_k
Pk来表示：
P
k
=
c
o
v
(
x
k
^
)
=
c
o
v
(
A
x
k
−
1
^
+
B
u
k
−
1
^
+
w
k
−
1
)
=
A
c
o
v
(
x
k
−
1
^
)
A
T
+
Q
=
A
P
k
−
1
A
T
+
Q
\begin{aligned} P_k&=cov(\hat{x_k})\\ &=cov(A\hat{x_{k-1}}+B\hat{u_{k-1}}+w_{k-1})\\ &=Acov(\hat{x_{k-1}})A^T+Q\\ &=A P_{k-1} A^T+Q \end{aligned}
Pk=cov(xk^)=cov(Axk−1^+Buk−1^+wk−1)=Acov(xk−1^)AT+Q=APk−1AT+Q
z
k
^
\hat{z_k}
zk^的方差可以表示为：
c
o
v
(
z
k
^
)
=
c
o
v
(
H
x
k
^
)
=
H
c
o
v
(
x
k
^
)
H
T
=
H
P
k
H
T
\begin{aligned} cov(\hat{z_k})&=cov(H\hat{x_k})\\ &=Hcov(\hat{x_k})H^T\\ &=HP_kH^T \end{aligned}
cov(zk^)=cov(Hxk^)=Hcov(xk^)HT=HPkHT
因此，
z
k
^
∼
N
(
H
x
k
^
,
H
P
k
H
T
)
\hat{z_k}\sim N(H\hat{x_k},HP_kH^T)
zk^∼N(Hxk^,HPkHT)
第二步：测量数值
z
k
：测量值
z_k：测量值
zk：测量值
z
k
∼
N
(
z
k
，
R
)
z_k\sim N(z_k，R)
zk∼N(zk，R)
第三步：寻找最优估计
现在有两个分布，一个是估计值的分布，一个是测量值的分布，最优解的分布可以通过将这两个分布相乘得到。
假设现在有
X
0
∼
N
(
μ
0
,
Σ
0
)
,
X
1
∼
N
(
μ
1
,
Σ
1
)
X_0\sim N(\mu_0, \Sigma_0), X_1\sim N(\mu_1, \Sigma_1)
X0∼N(μ0,Σ0),X1∼N(μ1,Σ1)，则
X
0
X
1
∼
(
μ
0
+
K
(
μ
1
−
μ
0
)
,
Σ
0
−
K
Σ
0
)
K
=
Σ
0
Σ
0
+
Σ
1
X_0X_1\sim (\mu_0 + K(\mu_1-\mu_0),\Sigma_0 - K\Sigma_0 )\\ K = \frac{\Sigma_0}{\Sigma_0+\Sigma_1}
X0X1∼(μ0+K(μ1−μ0),Σ0−KΣ0)K=Σ0+Σ1Σ0
带入
z
k
^
∼
N
(
H
x
k
^
,
H
P
k
H
T
)
\hat{z_k}\sim N(H\hat{x_k},HP_kH^T)
zk^∼N(Hxk^,HPkHT) 和
z
k
∼
N
(
z
k
，
R
)
z_k\sim N(z_k，R)
zk∼N(zk，R)得到
K
=
Σ
0
Σ
0
+
Σ
1
=
H
P
k
H
T
H
P
k
H
T
+
R
μ
′
=
μ
0
+
K
(
μ
1
−
μ
0
)
=
H
x
k
^
+
H
P
k
H
T
H
P
k
H
T
+
R
(
z
k
−
H
x
k
^
)
=
H
x
k
^
′
（
1
）
Σ
′
=
Σ
0
−
K
Σ
0
=
H
P
k
H
T
−
H
P
k
H
T
H
P
k
H
T
+
R
H
P
k
H
T
=
H
P
k
′
H
T
（
2
）
\begin{aligned} K&=\frac{\Sigma_0}{\Sigma_0+\Sigma_1} =\frac{HP_kH^T}{HP_kH^T+R}\\ \mu'&=\mu_0 + K(\mu_1-\mu_0)\\ &=H\hat{x_k}+\frac{HP_kH^T}{HP_kH^T+R}(z_k-H\hat{x_k}) = H\hat{x_k}' &&（1）\\ \Sigma' &= \Sigma_0 - K\Sigma_0 \\ &= H P_k H^T-\frac{HP_kH^T}{HP_kH^T+R} H P_k H^T = H {P_k}' H^T&&（2） \end{aligned}
Kμ′Σ′=Σ0+Σ1Σ0=HPkHT+RHPkHT=μ0+K(μ1−μ0)=Hxk^+HPkHT+RHPkHT(zk−Hxk^)=Hxk^′=Σ0−KΣ0=HPkHT−HPkHT+RHPkHTHPkHT=HPk′HT（1）（2）
K
K
K 被称为卡尔曼增益，用于更新
x
k
^
\hat{x_k}
xk^ 和
p
k
^
\hat{p_k}
pk^ 至
x
k
^
′
\hat{x_k}'
xk^′ 和
p
k
^
′
\hat{p_k}'
pk^′。
用
z
k
^
′
\hat{z_k}'
zk^′ 表示最优估计测量值，则
z
k
^
′
\hat{z_k}'
zk^′ 的分布为
z
k
^
′
∼
N
(
H
x
k
′
^
,
H
P
k
′
H
T
)
\hat{z_k} '\sim N(H\hat{x_k'}, HP_k'H^T)
zk^′∼N(Hxk′^,HPk′HT)
式（1）左右同左乘
H
−
1
H^{-1}
H−1 得到
x
k
^
′
=
x
k
^
+
P
k
H
T
H
P
k
H
T
+
R
(
z
k
−
H
x
k
^
)
\hat{x_k}' = \hat{x_k} + \frac{P_kH^T}{HP_kH^T+R}(z_k-H\hat{x_k})
xk^′=xk^+HPkHT+RPkHT(zk−Hxk^)
式（2）左右同左乘
H
−
1
H^{-1}
H−1 再同右乘
H
T
−
1
{H^T}^{-1}
HT−1 得到
P
k
′
H
T
=
P
k
H
T
−
P
k
H
T
H
P
k
H
T
+
R
H
P
k
H
T
P
k
′
=
P
k
−
P
k
H
T
H
P
k
H
T
+
R
H
P
k
\begin{aligned} {P_k}' H^T &= P_k H^T-\frac{P_kH^T}{HP_kH^T+R} H P_k H^T\\ {P_k}' &= P_k-\frac{P_kH^T}{HP_kH^T+R} H P_k \end{aligned}
Pk′HTPk′=PkHT−HPkHT+RPkHTHPkHT=Pk−HPkHT+RPkHTHPk
至此，迭代结束，得到了最优估计
x
k
^
′
\hat{x_k}'
xk^′ 的分布。
x
k
^
′
∼
N
(
x
k
^
+
K
′
(
z
k
−
H
x
k
^
)
,
P
k
−
K
′
H
P
k
)
K
′
=
P
k
H
T
H
P
k
H
T
+
R
\hat{x_k}'\sim N(\hat{x_k} + K'(z_k-H\hat{x_k}) , P_k-K' H P_k ) \\ K' = \frac{P_kH^T}{HP_kH^T+R}
xk^′∼N(xk^+K′(zk−Hxk^),Pk−K′HPk)K′=HPkHT+RPkHT
x
k
^
′
\hat{x_k}'
xk^′ 和其协方差
P
k
′
{P_k}'
Pk′ 将被用于对
x
k
+
1
^
\hat{x_{k+1}}
xk+1^ 的预测，
P
k
′
{P_k}'
Pk′ 就是下一轮预测中的
P
k
+
1
{P_{k+1}}
Pk+1
在式(1)中， K K K 的作用是调整估计测量值和测量值的比例。当过程噪声较小时， H x k ^ H\hat{x_k} Hxk^ 更准确， K K K 更趋近于0， z k ^ ′ \hat{z_k} ' zk^′更趋近于 H x k ^ H\hat{x_k} Hxk^；当测量噪声较小时， z k z_k zk更准确， K K K 更趋近于1， z k ^ ′ \hat{z_k} ' zk^′更趋近于 z k z_k zk。
放两张图帮助理解
扫一扫
695
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
