1. 卡尔曼滤波器概述
卡尔曼滤波器解决的问题:
- 主观认识不准确:
人在对问题的分析和建模与现实场景总是存在差异。 - 客观测量不准确
这个世界上的所有测量仪器都不可能完全准确。
卡尔曼滤波适用的场景:线性高斯系统
1.1 卡尔曼滤波器结构图
1.2 名词解释
系统状态方程:
x
k
=
A
x
k
−
1
+
B
u
k
−
1
+
w
k
x_k = Ax_{k-1} + Bu_{k-1} + w_k
xk=Axk−1+Buk−1+wk
x
k
,
x
k
−
1
x_k, x_{k-1}
xk,xk−1:系统在
k
k
k和
k
−
1
k-1
k−1时刻的状态。
A
A
A:状态转移矩阵,系统由上一个状态转变为下一个状态的运算矩阵。
u
k
−
1
u_{k-1}
uk−1:系统在
k
−
1
k-1
k−1时刻的输入控制量。
B
B
B:控制矩阵,输入控制量参与下一状态的运算矩阵。
w
k
w_k
wk:过程噪声。
测量方程:
y
k
=
C
x
k
+
v
k
y_k = Cx_k + v_k
yk=Cxk+vk
y
k
y_k
yk:测量量。
x
k
x_k
xk:状态量。
v
k
v_k
vk:测量噪声。
1.3 参数分析
噪声参数
w
k
∈
N
(
0
,
Q
k
)
w_k \in N(0,Q_k)
wk∈N(0,Qk)
过程噪声满足
μ
=
0
,
σ
2
=
Q
k
\mu = 0 , \sigma^{2} = Q_k
μ=0,σ2=Qk的高斯分布。
v
k
∈
N
(
0
,
R
k
)
v_k \in N(0,R_k)
vk∈N(0,Rk)
测量噪声满足
μ
=
0
,
σ
2
=
R
k
\mu = 0 , \sigma^{2} = R_k
μ=0,σ2=Rk的高斯分布。
方差
一维标准差:
σ
\sigma
σ
二维协方差:
c
o
v
(
X
,
Y
)
cov(X,Y)
cov(X,Y)
c
o
v
(
X
,
Y
)
=
σ
X
σ
Y
cov(X,Y) = \sigma_X \sigma_Y
cov(X,Y)=σXσY
多维协方差矩阵
P
=
[
c
o
v
(
X
1
,
X
1
)
c
o
v
(
X
1
,
X
2
)
.
.
.
c
o
v
(
X
1
,
X
n
)
c
o
v
(
X
2
,
X
1
)
c
o
v
(
X
2
,
X
2
)
.
.
.
c
o
v
(
X
2
,
X
n
)
.
.
.
.
.
.
.
.
.
.
.
.
c
o
v
(
X
n
,
X
1
)
c
o
v
(
X
n
,
X
2
)
.
.
.
c
o
v
(
X
n
,
X
n
)
]
P = \begin{bmatrix} cov(X_1,X_1) & cov(X_1,X_2) &... & cov(X_1,X_n) \\ cov(X_2,X_1) & cov(X_2,X_2) &... & cov(X_2,X_n) \\ ... & ... &... & ... \\ cov(X_n,X_1) & cov(X_n,X_2) &... & cov(X_n,X_n) \\ \end{bmatrix}
P=
cov(X1,X1)cov(X2,X1)...cov(Xn,X1)cov(X1,X2)cov(X2,X2)...cov(Xn,X2)............cov(X1,Xn)cov(X2,Xn)...cov(Xn,Xn)
超参数
Q
,
R
∼
P
I
D
Q,R \sim PID
Q,R∼PID
Q
Q
Q:过程噪声方差矩阵
R
R
R:测量噪声方差矩阵
2. 卡尔曼滤波核心
2.1 卡尔曼公式
预测:
KaTeX parse error: {align} can be used only in display mode.
更新:
KaTeX parse error: {align} can be used only in display mode.
公式说明:
符号上若带有 ^ \hat{} ^则表示其为先验估计值,若带有 ˉ \bar{} ˉ则表示其为预测值。
x x x:系统状态量
K t K_t Kt:卡尔曼增益矩阵
2.2 卡尔曼公式的理解
实现过程:
使用上一时刻的最优结果预测当前时刻的值,同时使用当前时刻观测值修正当前值,得到当前时刻最优结果。
因而卡尔曼滤波分为两个部分,预测和更新。