原理作用
当系统建模存在偏差时,滤波器往往出现过度拟合,不能反映系统真实状态,均方误差阵
P
k
P_k
Pk并不能反映状态估计精度。
解决办法是修改系统噪声
Q
k
,
Q_k,
Qk,和量测噪声
R
k
R_k
Rk的权重,从而减少历史信息的权重,相对提高了新信息的权重,减小系统惯性,在建模不准情况下改善滤波性能。
系统状态空间
X
k
:
n
维
状
态
向
量
X_k:n维状态向量
Xk:n维状态向量
Z
k
:
m
维
测
量
向
量
Z_k:m维测量向量
Zk:m维测量向量
Φ
k
/
k
−
1
:
已
知
的
系
统
结
构
参
数
\Phi_{k/k-1}:已知的系统结构参数
Φk/k−1:已知的系统结构参数
Γ
k
/
k
−
1
:
已
知
的
系
统
结
构
参
数
,
分
别
为
n
×
l
阶
系
统
分
配
噪
声
\Gamma_{k/k-1}:已知的系统结构参数,分别为n×l阶系统分配噪声
Γk/k−1:已知的系统结构参数,分别为n×l阶系统分配噪声
H
k
:
已
知
的
系
统
结
构
参
数
,
分
别
为
m
×
n
阶
测
量
矩
阵
H_k:已知的系统结构参数,分别为m×n阶测量矩阵
Hk:已知的系统结构参数,分别为m×n阶测量矩阵
V
k
:
m
维
测
量
噪
声
,
高
斯
白
噪
声
,
服
从
正
太
分
布
V_k:m维测量噪声,高斯白噪声,服从正太分布
Vk:m维测量噪声,高斯白噪声,服从正太分布
W
k
−
1
:
m
维
系
统
噪
声
向
量
,
高
斯
白
噪
声
,
服
从
正
太
分
布
W_{k-1}:m维系统噪声向量,高斯白噪声,服从正太分布
Wk−1:m维系统噪声向量,高斯白噪声,服从正太分布
V
k
与
W
k
−
1
互
不
相
关
V_k与W_{k-1}互不相关
Vk与Wk−1互不相关
{
X
k
=
Φ
k
/
k
−
1
X
k
−
1
+
Γ
k
/
k
−
1
W
k
−
1
Z
k
=
H
k
X
k
+
V
k
s
t
.
{
E
[
W
k
]
=
0
,
E
[
W
k
W
j
T
]
=
Q
k
δ
k
j
Q
k
≥
0
E
[
V
k
]
=
0
,
E
[
V
k
V
j
T
]
=
R
k
δ
k
j
,
E
[
W
k
V
j
T
]
=
0
R
≥
0
\begin{cases} X_k=\Phi_{k/k-1}X_{k-1}+\Gamma_{k/k-1}W_{k-1}\\ Z_k=H_kX_k+V_k\\ \end{cases} \\ st. \\ \begin{cases} E[W_k]=0,E[W_kW_j^T]=Q_k\delta_{kj} &Q_k \geq 0\\ E[V_k]=0,E[V_kV_j^T]=R_k\delta_{kj},E[W_kV_j^T]=0&R\geq 0\\ \end{cases}
{Xk=Φk/k−1Xk−1+Γk/k−1Wk−1Zk=HkXk+Vkst.{E[Wk]=0,E[WkWjT]=QkδkjE[Vk]=0,E[VkVjT]=Rkδkj,E[WkVjT]=0Qk≥0R≥0
修改系统噪声 E [ W k W j T ] E[W_kW_j^T] E[WkWjT]和量测噪声 E [ V k V j T ] E[V_kV_j^T] E[VkVjT]
s
>
1
:
s
为
遗
忘
因
子
,
s
越
大
遗
忘
速
度
越
大
s > 1:s为遗忘因子,s越大遗忘速度越大
s>1:s为遗忘因子,s越大遗忘速度越大
E
[
W
k
W
j
T
]
=
s
N
−
k
+
1
Q
k
δ
k
j
E
[
V
k
V
j
T
]
=
s
N
−
k
R
k
δ
k
j
(
i
,
j
≤
N
)
E[W_kW_j^T]=s^{N-k+1}Q_k\delta_{kj} \\ E[V_kV_j^T]=s^{N-k}R_k\delta_{kj} (i,j\leq N)
E[WkWjT]=sN−k+1QkδkjE[VkVjT]=sN−kRkδkj(i,j≤N)
kalman滤波公式(n时刻遗忘滤波模型下的)
n时刻的滤波公式如下:
{
X
^
k
/
k
−
1
N
=
Φ
k
/
k
−
1
X
^
k
−
1
N
状
态
预
测
/
时
间
更
新
P
k
/
k
−
1
N
=
Φ
k
/
k
−
1
P
k
−
1
N
Φ
k
/
k
−
1
T
+
Γ
k
−
1
s
N
−
k
Q
k
−
1
Γ
k
−
1
T
状
态
预
测
均
方
差
阵
/
量
测
更
新
K
k
N
=
P
k
/
k
−
1
N
H
k
T
(
H
k
P
k
/
k
−
1
N
H
k
T
+
s
N
−
k
R
k
)
−
1
滤
波
增
益
X
^
k
N
=
(
I
−
K
k
H
k
)
X
^
k
/
k
−
1
N
+
K
k
N
Z
k
状
态
估
计
P
k
N
=
(
I
−
K
k
N
H
k
)
P
k
/
k
−
1
N
状
态
估
计
均
方
误
差
阵
\begin{cases} \hat X^N_{k/k-1}=\Phi_{k/k-1}\hat X^N_{k-1}&状态预测/时间更新\\ P^N_{k/k-1}=\Phi_{k/k-1}P^N_{k-1}\Phi^T_{k/k-1}+\Gamma_{k-1}s^{N-k}Q_{k-1}\Gamma_{k-1}^T&状态预测均方差阵/量测更新\\ K_k^N=P_{k/k-1}^NH_k^T(H_kP_{k/k-1}^NH_k^T+s^{N-k}R_k)^{-1}&滤波增益\\ \hat X_k^N=(I-K_kH_k)\hat X^N_{k/k-1}+K_k^NZ_k&状态估计\\ P_k^N=(I-K_k^NH_k)P^N_{k/k-1}&状态估计均方误差阵\\ \end{cases}
⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧X^k/k−1N=Φk/k−1X^k−1NPk/k−1N=Φk/k−1Pk−1NΦk/k−1T+Γk−1sN−kQk−1Γk−1TKkN=Pk/k−1NHkT(HkPk/k−1NHkT+sN−kRk)−1X^kN=(I−KkHk)X^k/k−1N+KkNZkPkN=(I−KkNHk)Pk/k−1N状态预测/时间更新状态预测均方差阵/量测更新滤波增益状态估计状态估计均方误差阵
简化:
令
X
^
k
/
k
−
1
∗
=
Φ
k
/
k
−
1
X
^
k
−
1
∗
;
X
^
k
∗
=
X
^
k
N
;
X
^
k
/
k
−
1
∗
=
X
^
k
/
k
−
1
∗
;
K
k
∗
=
K
k
N
\hat X^*_{k/k-1}=\Phi_{k/k-1}\hat X_{k-1}^*;\hat X_k^*=\hat X_k^N;\hat X^*_{k/k-1}=\hat X_{k/k-1}^*;K_k^*=K_k^N
X^k/k−1∗=Φk/k−1X^k−1∗;X^k∗=X^kN;X^k/k−1∗=X^k/k−1∗;Kk∗=KkN
{
X
^
k
/
k
−
1
∗
=
Φ
k
/
k
−
1
X
^
k
−
1
∗
状
态
预
测
/
时
间
更
新
P
k
/
k
−
1
∗
=
Φ
k
/
k
−
1
(
s
P
k
−
1
∗
)
Φ
k
/
k
−
1
T
+
Γ
k
−
1
Q
k
−
1
Γ
k
−
1
T
状
态
预
测
均
方
差
阵
/
量
测
更
新
K
k
∗
=
P
k
/
k
−
1
∗
H
k
T
(
H
k
P
k
/
k
−
1
∗
H
k
T
+
R
k
)
−
1
滤
波
增
益
X
^
k
∗
=
(
I
−
K
k
H
k
)
X
^
k
/
k
−
1
∗
+
K
k
∗
Z
k
状
态
估
计
P
k
∗
=
(
I
−
K
k
∗
H
k
)
P
k
/
k
−
1
∗
状
态
估
计
均
方
误
差
阵
\begin{cases} \hat X^*_{k/k-1}=\Phi_{k/k-1}\hat X^*_{k-1}&状态预测/时间更新\\ P^*_{k/k-1}=\Phi_{k/k-1}(sP^*_{k-1})\Phi^T_{k/k-1}+\Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^T&状态预测均方差阵/量测更新\\ K_k^*=P_{k/k-1}^*H_k^T(H_kP_{k/k-1}^*H_k^T+R_k)^{-1}&滤波增益\\ \hat X_k^*=(I-K_kH_k)\hat X^*_{k/k-1}+K_k^*Z_k&状态估计\\ P_k^*=(I-K_k^*H_k)P^*_{k/k-1}&状态估计均方误差阵\\ \end{cases}
⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧X^k/k−1∗=Φk/k−1X^k−1∗Pk/k−1∗=Φk/k−1(sPk−1∗)Φk/k−1T+Γk−1Qk−1Γk−1TKk∗=Pk/k−1∗HkT(HkPk/k−1∗HkT+Rk)−1X^k∗=(I−KkHk)X^k/k−1∗+Kk∗ZkPk∗=(I−Kk∗Hk)Pk/k−1∗状态预测/时间更新状态预测均方差阵/量测更新滤波增益状态估计状态估计均方误差阵