卡尔曼滤波公式推导
Posted herr_edoc
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了卡尔曼滤波公式推导相关的知识，希望对你有一定的参考价值。
一、背景知识
卡尔曼滤波器和其衍生出来的一系列滤波器被验证是行之有效的多源数据融合方法，在工程上有广泛的应用。应用的场景主要分两种：1. 预测无法直接测量的状态量；2.融合多源的信息得出更加准确的状态量。大家都知道卡尔曼滤波器分为预测和更新两个过程，一共有5个公式，但是很多人不知道公式是怎么推导出来的。这两篇文章会用两种方式详细推导卡尔曼滤波器的公式。
卡尔曼滤波器的5个经典公式如下：
预测（时间更新过程）
x
^
k
−
=
A
k
x
^
k
−
1
+
B
k
u
k
+
w
k
\\hat x_k^-=A_k\\hat x_k-1+B_ku_k+w_k
x^k−=Akx^k−1+Bkuk+wk
P
^
k
−
=
A
k
P
k
−
1
A
k
T
+
Q
k
\\hat P_k^-=A_kP_k-1A_k^T+Q_k
P^k−=AkPk−1AkT+Qk
更新（状态更新过程）
K
k
=
P
k
−
H
K
T
(
H
k
P
k
−
H
k
T
+
R
k
)
−
1
K_k=P_k^-H_K^T(H_kP_k^-H_k^T+R_k)^-1
Kk=Pk−HKT(HkPk−HkT+Rk)−1
x
^
k
=
x
^
k
−
+
K
k
(
z
k
−
H
k
x
^
k
−
)
\\hat x_k=\\hat x_k^-+K_k(z_k-H_k\\hat x_k^-)
x^k=x^k−+Kk(zk−Hkx^k−)
P
k
=
P
k
−
−
K
k
H
k
P
k
−
P_k=P_k^--K_kH_kP_k^-
Pk=Pk−−KkHkPk−
此外，在测量状态时有：
x
k
=
z
k
+
v
k
x_k=z_k+v_k
xk=zk+vk
以下是各个变量的含义：
x
k
x_k
xk: 状态向量的真实值
x
^
k
−
\\hat x_k^-
x^k−: 状态向量的先验估计值
x
^
k
−
1
\\hat x_k-1
x^k−1: 上个时刻状态向量的后验估计值
x
^
k
\\hat x_k
x^k: 当前时刻状态向量的后验估计值
A
k
A_k
Ak: 状态转移矩阵
B
k
B_k
Bk: 状态控制矩阵
u
k
u_k
uk: 状态控制向量
w
k
w_k
wk: 过程噪声，符合正态分布
w
k
∼
N
(
0
,
Q
k
)
w_k \\sim N(0, Q_k)
wk∼N(0,Qk)
P
^
k
−
\\hat P_k^-
P^k−: 预测协方差矩阵的先验估计值
P
k
−
1
P_k-1
Pk−1: 上个时刻预测协方差矩阵的后验估计值
P
k
P_k
Pk: 当前时刻预测协方差矩阵的后验估计值
Q
k
Q_k
Qk: 过程噪声协方差矩阵
z
k
z_k
zk: 测量向量
v
k
v_k
vk: 测量噪声，符合正态分布
v
k
∼
N
(
0
,
R
k
)
v_k \\sim N(0, R_k)
vk∼N(0,Rk)
R
k
R_k
Rk: 测量噪声的协方差矩阵
H
k
H_k
Hk: 状态向量到测量向量的转换矩阵
二、卡尔曼滤波公式的推导
在预测和更新阶段分别能得到两个近似状态向量真实值的值，记为 x ^ p r e d i c t = H k x ^ k − \\hat x^predict=H_k\\hat x_k^- x^pred以上是关于卡尔曼滤波公式推导的主要内容，如果未能解决你的问题，请参考以下文章