卡尔曼滤波
卡尔曼滤波适用于线性高斯系统，即系统满足叠加性、齐次性，噪声满足正态分布。其使用上一次的最优结果预测当前的值（先验估计），同时使用观测值修正当前值，得到最优结果。
卡尔曼、粒子滤波实际应该叫做估计器（Estimator），估计当前值叫滤波（Filtering），估计过去叫平滑（Smoothing），估计未来叫预测（Predicting）。
卡尔曼滤波利用目标的动态信息，设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计（滤波），也可以是对于将来位置的估计（预测），也可以是对过去位置的估计（插值或平滑）。
Kalman Filter
1. 状态空间模型
卡尔曼滤波假设系统状态与噪声是高斯的，可以用均值和方差来描述。则上述状态空间模型可以被表述为：
2. 5个公式
2.1 预测
根据上一时刻(
k
−
1
k-1
k−1 时刻) 的后验估计值来估计当前时刻(
k
k
k 时刻) 的状态，得到
k
k
k 时刻的先验估计值;
x
^
k
‾
=
A
x
^
k
−
1
+
B
u
k
−
1
\hat x_{\overline k} = A\hat x_{k-1} + Bu_{k-1}
x^k=Ax^k−1+Buk−1
-
x
^
k
‾
\hat x_{\overline k}
x^k：
k
k
k 时刻的先验状态估计值，即根据上一时刻（
k
−
1
k-1
k−1时刻）的最优估计预测的
k
k
k 时刻的结果。
-
x
^
k
−
1
\hat x_{k-1}
x^k−1 表示
k
−
1
k-1
k−1 时刻的后验状态估计值，也叫最优估计，是滤波的结果之一。
-
A
A
A：状态转移矩阵，实际上是对目标状态转换的一种猜想模型。
-
u
k
−
1
u_{k-1}
uk−1：控制向量
-
B
B
B：是将控制向量
u
k
−
1
u_{k-1}
uk−1转换为状态的矩阵
P
k
‾
=
A
P
k
−
1
A
T
+
Q
P_{\overline k} = AP_{k-1}A^T + Q
Pk=APk−1AT+Q
-
P
k
‾
P_{\overline k}
Pk：
k
k
k 时刻的先验估计协方差（
x
^
k
‾
\hat x_{\overline k}
x^k 的协方差)
-
A
A
A：状态转移矩阵，实际上是对目标状态转换的一种猜想模型。
-
P
k
−
1
P_{k-1}
Pk−1：
k
−
1
k-1
k−1 时刻后验估计协方差（即
x
^
k
−
1
\hat x_{k-1}
x^k−1 的协方差，表示状态的不确定度），是滤波的结果之一。
-
Q
Q
Q: 过程激励噪声协方差（系统过程的协方差）。该参数被用来表示状态转换矩阵与实际过程之间的误差。因为我们无法直接观测到过程信号， 所以
Q
Q
Q 的取值是很难确定的。是卡尔曼滤波器用于估计离散时间过程的状态变量，也叫预测模型本身带来的噪声。状态转移协方差矩阵
2.2 更新
使用当前时刻的测量值来更正预测阶段先验估计值，得到当前时刻的后验估计值。
K
k
=
P
k
‾
H
T
H
P
k
‾
H
T
+
R
K_k = \frac{P_{\overline k }H^T}{HP_{\overline k}H^T + R}
Kk=HPkHT+RPkHT
-
K
k
K_k
Kk：滤波增益矩阵，是滤波的中间计算结果，卡尔曼增益，或卡尔曼系数。
-
P
k
‾
P_{\overline k}
Pk：
k
k
k 时刻的先验估计协方差（
x
^
k
‾
\hat x_{\overline k}
x^k 的协方差)
-
H
H
H：是状态变量到测量（观测）的转换矩阵，表示将状态和观测连接起来的关系，卡尔曼滤波里为线性关系，它负责将 m 维的测量值转换到 n 维，使之符合状态变量的数学形式，是滤波的前提条件之一。
-
R
R
R：测量噪声协方差。滤波器实际实现时，测量噪声协方差 R一般可以观测得到，是滤波器的已知条件。
x
^
k
=
x
^
k
‾
+
K
k
(
z
k
−
H
x
^
k
‾
)
\hat x_k = \hat x_{\overline k} + K_k(z_k- H\hat x_{\overline k})
x^k=x^k+Kk(zk−Hx^k)
-
x
^
k
\hat x_{k}
x^k 表示
k
k
k 时刻的后验状态估计值，也叫最优估计，是滤波的结果之一。
-
x
^
k
‾
\hat x_{\overline k}
x^k：
k
k
k 时刻的先验状态估计值，即根据上一时刻（
k
−
1
k-1
k−1时刻）的最优估计预测的
k
k
k 时刻的结果。
-
K
k
K_k
Kk：滤波增益矩阵，是滤波的中间计算结果，卡尔曼增益，或卡尔曼系数。
-
z
k
z_k
zk：测量值（观测值），是滤波的输入。
-
H
H
H：是状态变量到测量（观测）的转换矩阵，表示将状态和观测连接起来的关系。
-
z
k
−
H
x
^
k
‾
z_k- H\hat x_{\overline k}
zk−Hx^k：实际观测和预测观测的残差，和卡尔曼增益一起修正先验（预测），得到后验。
P
k
=
(
I
−
K
k
H
)
P
k
‾
P_k = (I - K_kH) P_{\overline k}
Pk=(I−KkH)Pk
-
P
k
P_{k}
Pk：
k
k
k 时刻后验估计协方差（即
x
^
k
\hat x_{k}
x^k 的协方差，表示状态的不确定度），是滤波的结果之一。
-
I
I
I：单位矩阵
-
K
k
K_k
Kk：卡尔曼增益，或卡尔曼系数。
-
H
H
H：是状态变量到测量（观测）的转换矩阵。
-
P
k
‾
P_{\overline k}
Pk：
k
k
k 时刻的先验估计协方差（
x
^
k
‾
\hat x_{\overline k}
x^k 的协方差)
3. 代码实现
prediction
x
′
=
F
x
+
u
P
′
=
F
P
F
T
+
Q
x^\prime = Fx + u \\ P^\prime = FPF^T + Q
x′=Fx+uP′=FPFT+Q
update
y
=
z
−
H
x
′
S
=
H
P
′
H
T
+
R
K
=
P
′
H
T
S
−
1
x
=
x
′
+
K
y
P
=
(
I
−
K
H
)
P
′
y = z-Hx^\prime \\ S = HP^\prime H^T + R \\ K = P^\prime H^T S^{-1} \\ x = x^\prime + Ky \\ P = (I - KH)P^\prime
y=z−Hx′S=HP′HT+RK=P′HTS−1x=x′+KyP=(I−KH)P′
code
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
