卡尔曼滤波
一.卡尔曼滤波使用
1.线性系统模型：
{
状态方程：
X
(
i
)
=
F
X
(
i
−
1
)
+
B
U
(
i
−
1
)
+
W
(
i
)
观测方程：
Y
(
i
)
=
H
X
(
i
)
+
V
(
i
)
\left\{ \begin{aligned} &状态方程：X(i)=FX(i-1)+BU(i-1)+W(i)\\ &观测方程： Y(i)=HX(i)+V(i) \end{aligned} \right.
{状态方程：X(i)=FX(i−1)+BU(i−1)+W(i)观测方程：Y(i)=HX(i)+V(i)
F
F
F为状态转移矩阵，B为控制矩阵，H为观测矩阵，W为输入的白噪声，且其方差为Q，V为观测噪声，且其方差为R。
2.卡尔曼滤波递推过程
核心：预测+更新
变量名称：
{
状态变量先验估计值：
X
^
(
i
∣
i
−
1
)
=
X
^
i
−
协方差变量先验估计值：
P
(
i
∣
i
−
1
)
=
P
i
−
卡尔曼增益：
K
(
i
)
状态变量最优估计值：
X
^
(
i
∣
i
)
=
X
^
i
协方差变量最优估计值：
P
(
i
∣
i
)
=
P
i
\left\{ \begin{aligned} &状态变量先验估计值：\hat{X}(i|i-1)=\hat{X}^-_i\\ &协方差变量先验估计值：P(i|i-1)=P^-_i\\ &卡尔曼增益：K(i)\\ &状态变量最优估计值：\hat{X}(i|i)=\hat{X}_i\\ &协方差变量最优估计值：P(i|i)=P_i \end{aligned} \right.
⎩
⎨
⎧状态变量先验估计值：X^(i∣i−1)=X^i−协方差变量先验估计值：P(i∣i−1)=Pi−卡尔曼增益：K(i)状态变量最优估计值：X^(i∣i)=X^i协方差变量最优估计值：P(i∣i)=Pi
(1)预测过程：利用上一次的最优结果预测当前的状态值：
{
X
^
(
i
∣
i
−
1
)
=
F
X
^
(
i
−
1
∣
i
−
1
)
+
B
U
(
i
−
1
)
P
(
i
∣
i
−
1
)
=
F
P
(
i
−
1
∣
i
−
1
)
F
T
+
Q
\left\{ \begin{aligned} &\hat{X}(i|i-1)=F\hat{X}(i-1|i-1)+BU(i-1)\\ &P(i|i-1)=FP(i-1|i-1)F^T+Q \end{aligned} \right.
{X^(i∣i−1)=FX^(i−1∣i−1)+BU(i−1)P(i∣i−1)=FP(i−1∣i−1)FT+Q
(2)更新过程：利用观测值和预测值更新得到当前的最优状态值（利用观测值修正预测值）：
{
K
(
i
)
=
P
i
(
i
∣
i
−
1
)
H
T
(
H
P
i
(
i
∣
i
−
1
)
H
T
+
R
)
−
1
X
^
(
i
)
=
X
^
(
i
∣
i
−
1
)
+
K
(
i
)
(
Y
(
i
)
−
H
X
^
(
i
∣
i
−
1
)
)
P
(
i
)
=
(
I
−
K
(
i
)
H
)
P
\left\{ \begin{aligned} &K(i)=P_i(i|i-1)H^T(HP_i(i|i-1)H^T+R)^{-1}\\ &\hat{X}(i)=\hat{X}(i|i-1)+K(i)(Y(i)-H\hat{X}(i|i-1))\\ &P(i)=(I-K(i)H)P \end{aligned} \right.
⎩
⎨
⎧K(i)=Pi(i∣i−1)HT(HPi(i∣i−1)HT+R)−1X^(i)=X^(i∣i−1)+K(i)(Y(i)−HX^(i∣i−1))P(i)=(I−K(i)H)P
949
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
