噪声相关条件下的Kalman滤波
理想状态下的kalman需要系统噪声与测量噪声之间部不相关,如果测量噪声与系统噪声相关需要进行处理
噪声相关条件下的系统状态方程
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
]
=
S
k
δ
k
j
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]=S_k\delta_{kj}&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]=SkδkjQk≥0R≥0系统方程测量方程
## 噪声相关条件下的Kalman滤波
重新构造测量方程,使测量误差与系统误差协方差为0
构造矩阵
J
k
−
1
J_{k-1}
Jk−1满足如下条件:
0
=
J
k
−
1
(
Z
k
−
1
−
H
k
−
1
X
k
−
1
−
V
k
−
1
)
0=J_{k-1}(Z_{k-1}-H_{k-1}X_{k-1}-V_{k-1})
0=Jk−1(Zk−1−Hk−1Xk−1−Vk−1)
将条件带入系统方程:
X
k
=
Φ
k
/
k
−
1
X
k
−
1
+
Γ
k
/
k
−
1
W
k
−
1
+
J
k
−
1
(
Z
k
−
1
−
H
k
−
1
X
k
−
1
−
V
k
−
1
)
=
Φ
k
/
k
−
1
∗
X
k
−
1
+
J
k
−
1
Z
k
−
1
+
W
k
−
1
∗
X_k=\Phi_{k/k-1}X_{k-1}+\Gamma_{k/k-1}W_{k-1}+J_{k-1}(Z_{k-1}-H_{k-1}X_{k-1}-V_{k-1}) \\ =\Phi^*_{k/k-1}X_{k-1}+J_{k-1}Z_{k-1}+W_{k-1}^*
Xk=Φk/k−1Xk−1+Γk/k−1Wk−1+Jk−1(Zk−1−Hk−1Xk−1−Vk−1)=Φk/k−1∗Xk−1+Jk−1Zk−1+Wk−1∗
得到:
{
X
k
=
Φ
k
/
k
−
1
∗
X
k
−
1
+
J
k
−
1
Z
k
−
1
+
W
k
−
1
∗
Φ
k
/
k
−
1
∗
=
Φ
k
/
k
−
1
−
J
k
−
1
H
k
−
1
W
k
−
1
∗
=
Γ
k
−
1
W
k
−
1
−
J
k
−
1
V
k
−
1
\begin{cases} X_k=\Phi^*_{k/k-1}X_{k-1}+J_{k-1}Z_{k-1}+W_{k-1}^*\\ \Phi^*_{k/k-1}=\Phi_{k/k-1}-J_{k-1}H_{k-1}\\ W_{k-1}^*=\Gamma_{k-1}W_{k-1}-J_{k-1}V_{k-1}\\ \end{cases}
⎩⎪⎨⎪⎧Xk=Φk/k−1∗Xk−1+Jk−1Zk−1+Wk−1∗Φk/k−1∗=Φk/k−1−Jk−1Hk−1Wk−1∗=Γk−1Wk−1−Jk−1Vk−1
求期望:
{
E
[
W
k
∗
]
=
E
[
Γ
k
−
1
W
k
−
1
−
J
k
−
1
V
k
−
1
]
=
0
E
[
W
k
∗
W
j
∗
T
]
=
(
Γ
k
Q
k
Γ
k
T
+
J
k
R
k
J
k
T
−
Γ
k
S
k
J
k
T
−
J
k
S
k
T
Γ
k
T
)
δ
k
j
E
[
W
k
∗
V
j
T
]
=
(
Γ
k
S
k
−
J
k
R
k
)
δ
k
j
\begin{cases} E[W_k^*]=E[\Gamma_{k-1}W_{k-1}-J_{k-1}V_{k-1}]=0\\ E[W_k^*W_j^{*T}]=(\Gamma_kQ_k\Gamma^T_k+J_kR_kJ_k^T-\Gamma_kS_kJ_k^T-J_kS_k^T\Gamma_k^T)\delta_{kj}\\ E[W_k^*V_j^T]=(\Gamma_kS_k-J_kR_k)\delta_{kj}\\ \end{cases}
⎩⎪⎨⎪⎧E[Wk∗]=E[Γk−1Wk−1−Jk−1Vk−1]=0E[Wk∗Wj∗T]=(ΓkQkΓkT+JkRkJkT−ΓkSkJkT−JkSkTΓkT)δkjE[Wk∗VjT]=(ΓkSk−JkRk)δkj
要使测量误差与系统误差不相关:
E
[
W
k
∗
V
j
T
]
=
(
Γ
k
S
k
−
J
k
R
k
)
δ
k
j
=
0
E[W_k^*V_j^T]=(\Gamma_kS_k-J_kR_k)\delta_{kj}=0
E[Wk∗VjT]=(ΓkSk−JkRk)δkj=0
求得
S
k
S_k
Sk
S
k
=
Γ
k
S
k
R
k
−
1
S_k=\Gamma_kS_kR_k^{-1}
Sk=ΓkSkRk−1
重新计算方差阵为:
E
[
W
k
∗
W
j
∗
T
]
=
(
Γ
k
Q
k
Γ
k
T
−
J
k
R
k
J
k
T
)
δ
k
j
E[W_k^*W_j^{*T}]=(\Gamma_kQ_k\Gamma_k^T-J_kR_kJ_k^T)\delta_{kj}
E[Wk∗Wj∗T]=(ΓkQkΓkT−JkRkJkT)δkj
Q
k
∗
=
Γ
k
Q
k
Γ
k
T
−
J
k
R
k
J
k
T
Q_k^*=\Gamma_kQ_k\Gamma_k^T-J_kR_kJ_k^T
Qk∗=ΓkQkΓkT−JkRkJkT
得到新的状态空间方程为:
{
X
k
=
Φ
k
/
k
−
1
∗
X
k
−
1
+
J
k
−
1
Z
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}+J_{k-1}Z_{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_k^*V_j^T]=0&R\geq 0&测量方程\\ \end{cases}
{Xk=Φk/k−1∗Xk−1+Jk−1Zk−1+Wk−1∗Zk=HkXk+Vkst.{E[Wk∗]=0,E[Wk∗Wj∗T]=Qk∗δkjE[Vk]=0,E[VkVjT]=Rkδkj,E[Wk∗VjT]=0Qk∗≥0R≥0系统方程测量方程
得到kalman滤波方程为
{
X
^
k
/
k
−
1
=
Φ
k
/
k
−
1
∗
X
^
k
−
1
+
J
k
−
1
Z
k
−
1
状
态
一
步
预
测
P
k
/
k
−
1
=
Φ
k
/
k
−
1
∗
P
k
−
1
Φ
k
/
k
−
1
∗
T
+
Q
k
−
1
∗
状
态
一
步
预
测
均
方
差
阵
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}+J_{k-1}Z_{k-1}&状态一步预测\\ P_{k/k-1}=\Phi^*_{k/k-1}P_{k-1}\Phi^{*T}_{k/k-1}+Q_{k-1}^*&状态一步预测均方差阵\\ 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_kZ_k&状态估计\\ P_k=(I-K_kH_k)P_{k/k-1}&状态估计均方误差阵\\ \end{cases}
⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧X^k/k−1=Φk/k−1∗X^k−1+Jk−1Zk−1Pk/k−1=Φk/k−1∗Pk−1Φk/k−1∗T+Qk−1∗Kk=Pk/k−1HkT(HkPk/k−1HkT−Rk)−1X^k=(I−KkHk)X^k/k−1+KkZkPk=(I−KkHk)Pk/k−1状态一步预测状态一步预测均方差阵滤波增益状态估计状态估计均方误差阵
系统噪声为有色噪声的Kalman滤波
系统噪声为有色噪声的系统状态方程
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
不
相
关
:
Γ
k
W
k
=
[
Γ
w
,
k
Γ
c
,
k
]
[
W
w
,
k
W
c
,
k
]
V_k与W_{k-1}不相关:\Gamma_{k}W_{k}= \left[\begin{matrix}\Gamma_{w,k}&\Gamma_{c,k} \end{matrix}\right] \left[\begin{matrix}W_{w,k}\\W_{c,k} \end{matrix}\right]
Vk与Wk−1不相关:ΓkWk=[Γw,kΓc,k][Ww,kWc,k]
{
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
w
,
k
]
=
0
,
E
[
W
w
,
k
W
w
,
j
T
]
=
Q
w
,
k
δ
k
j
,
E
[
W
w
,
k
W
c
,
j
T
]
=
0
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_{w,k}]=0,E[W_{w,k}W_{w,j}^T]=Q_{w,k}\delta_{kj},E[W_{w,k}W_{c,j}^T]=0&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[Ww,k]=0,E[Ww,kWw,jT]=Qw,kδkj,E[Ww,kWc,jT]=0E[Vk]=0,E[VkVjT]=Rkδkj,E[WkVjT]=0Qk≥0R≥0系统方程测量方程
系统噪声表示为有限维状态
W
c
,
k
′
:
为
W
c
,
k
的
拓
展
维
数
W_{c,k}' :为W_{c,k}的拓展维数
Wc,k′:为Wc,k的拓展维数
ξ
c
,
k
:
零
均
值
高
斯
白
噪
声
\xi_{c,k} :零均值高斯白噪声
ξc,k:零均值高斯白噪声
[
W
c
,
k
W
c
,
k
′
]
=
[
Π
k
/
k
−
1
11
Π
k
/
k
−
1
12
Π
k
/
k
−
1
21
Π
k
/
k
−
1
22
]
[
W
c
,
k
−
1
W
c
,
k
−
1
′
]
+
[
ξ
c
,
k
ξ
c
,
k
′
]
E
[
[
ξ
k
ξ
k
′
]
]
=
0
,
E
[
[
ξ
k
ξ
k
′
]
[
ξ
j
ξ
j
′
]
T
]
=
[
Q
c
,
k
0
0
Q
c
,
k
′
]
δ
k
,
j
\left[\begin{matrix} W_{c,k}\\ W_{c,k}' \end{matrix}\right]=\left[\begin{matrix} \Pi_{k/k-1}^{11}&\Pi_{k/k-1}^{12}\\ \Pi_{k/k-1}^{21}&\Pi_{k/k-1}^{22} \end{matrix}\right] \left[\begin{matrix}W_{c,k-1}\\ W_{c,k-1}' \end{matrix}\right]+ \left[\begin{matrix} \xi_{c,k}\\ \xi_{c,k}' \end{matrix}\right]\\ E[\left[\begin{matrix} \xi_{k}\\ \xi_{k}' \end{matrix}\right]]=0,E[\left[\begin{matrix} \xi_{k}\\ \xi_{k}' \end{matrix}\right]\left[\begin{matrix} \xi_{j}\\ \xi_{j}' \end{matrix}\right]^T]=\left[\begin{matrix} Q_{c,k}&0\\ 0&Q_{c,k}' \end{matrix}\right]\delta_{k,j}
[Wc,kWc,k′]=[Πk/k−111Πk/k−121Πk/k−112Πk/k−122][Wc,k−1Wc,k−1′]+[ξc,kξc,k′]E[[ξkξk′]]=0,E[[ξkξk′][ξjξj′]T]=[Qc,k00Qc,k′]δk,j
系统噪声表示为有限维状态带入状态空间
{
X
k
a
=
Φ
k
/
k
−
1
a
X
k
−
1
a
+
Γ
k
/
k
−
1
a
W
k
−
1
a
Z
k
=
H
k
a
X
k
a
+
V
k
s
t
.
{
E
[
W
w
,
k
a
]
=
0
,
E
[
W
w
,
k
a
W
w
,
j
a
T
]
=
Q
w
,
k
δ
k
j
=
d
i
a
g
(
Q
w
,
k
,
Q
c
,
k
,
Q
c
,
k
′
)
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^a=\Phi_{k/k-1}^aX^a_{k-1}+\Gamma^a_{k/k-1}W^a_{k-1}&\\ Z_k=H^a_kX^a_k+V_k\\ \end{cases} \\ st. \\ \begin{cases} E[W^a_{w,k}]=0,E[W^a_{w,k}W_{w,j}^{aT}]=Q_{w,k}\delta_{kj}=diag(Q_{w,k},Q_{c,k},Q_{c,k}')&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}
{Xka=Φk/k−1aXk−1a+Γk/k−1aWk−1aZk=HkaXka+Vkst.{E[Ww,ka]=0,E[Ww,kaWw,jaT]=Qw,kδkj=diag(Qw,k,Qc,k,Qc,k′)E[Vk]=0,E[VkVjT]=Rkδkj,E[WkVjT]=0Qk≥0R≥0系统方程测量方程
X
k
a
=
[
X
k
W
c
,
k
W
c
,
k
′
]
,
Φ
k
/
k
−
1
a
=
[
Φ
k
/
k
−
1
Γ
c
,
k
−
1
0
0
Π
k
/
k
−
1
k
/
k
−
1
11
Π
k
/
k
−
1
k
/
k
−
1
12
0
Π
k
/
k
−
1
k
/
k
−
1
21
Π
k
/
k
−
1
k
/
k
−
1
22
]
Γ
k
a
=
[
Γ
w
,
k
0
0
0
I
0
0
0
I
]
,
W
k
a
=
[
W
w
,
k
ξ
k
ξ
k
′
]
,
H
k
a
=
[
H
k
0
0
]
X_k^a=\left[\begin{matrix} X_k\\ W_{c,k}\\ W_{c,k}' \end{matrix}\right],\Phi_{k/k-1}^a= \left[\begin{matrix} \Phi_{k/k-1}&\Gamma_{c,k-1}&0\\ 0&\Pi_{k/k-1 \ k/k-1}^{11}&\Pi_{k/k-1 \ k/k-1}^{12}\\ 0&\Pi_{k/k-1 \ k/k-1}^{21}&\Pi_{k/k-1 \ k/k-1}^{22}\\ \end{matrix}\right] \\ \Gamma_k^a=\left[\begin{matrix} \Gamma_{w,k}&0&0\\ 0&I&0\\ 0&0&I \end{matrix}\right],W_k^a= \left[\begin{matrix} W_{w,k}\\\xi_k\\\xi_k' \end{matrix}\right],H_k^a= \left[\begin{matrix} H_k&0&0\\ \end{matrix}\right]
Xka=⎣⎡XkWc,kWc,k′⎦⎤,Φk/k−1a=⎣⎡Φk/k−100Γc,k−1Πk/k−1 k/k−111Πk/k−1 k/k−1210Πk/k−1 k/k−112Πk/k−1 k/k−122⎦⎤Γka=⎣⎡Γw,k000I000I⎦⎤,Wka=⎣⎡Ww,kξkξk′⎦⎤,Hka=[Hk00]
测量噪声为有色噪声的Kalman滤波
将有色噪声白化,假设测量误差
V
k
V_k
Vk仅有一部分分量是白噪声
{
X
k
=
Φ
k
/
k
−
1
X
k
−
1
+
Γ
k
/
k
−
1
W
k
−
1
Z
k
=
H
k
X
k
+
V
k
\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} \\
{Xk=Φk/k−1Xk−1+Γk/k−1Wk−1Zk=HkXk+Vk
状态增广法
V
k
=
[
Θ
w
,
k
Θ
c
,
k
]
[
V
w
,
k
V
c
,
k
]
E
[
V
w
,
k
]
=
0
,
E
[
V
w
,
k
V
w
,
j
T
]
=
R
w
,
k
δ
k
j
,
E
[
V
w
,
k
V
c
,
j
T
]
=
0
V_k= \left[\begin{matrix}\Theta_{w,k}&\Theta_{c,k} \end{matrix}\right] \left[\begin{matrix}V_{w,k}\\V_{c,k} \end{matrix}\right] \\ E[V_{w,k}]=0,E[V_{w,k}V_{w,j}^T]=R_{w,k}\delta_{kj},E[V_{w,k}V_{c,j}^T]=0
Vk=[Θw,kΘc,k][Vw,kVc,k]E[Vw,k]=0,E[Vw,kVw,jT]=Rw,kδkj,E[Vw,kVc,jT]=0
系统噪声表示为有限维状态
[
V
c
,
k
V
c
,
k
′
]
=
[
Ψ
k
/
k
−
1
11
Ψ
k
/
k
−
1
12
Ψ
k
/
k
−
1
21
Ψ
k
/
k
−
1
22
]
[
V
c
,
k
−
1
V
c
,
k
−
1
′
]
+
[
ξ
c
,
k
ξ
c
,
k
′
]
E
[
[
ξ
k
ξ
k
′
]
]
=
0
,
E
[
[
ξ
k
ξ
k
′
]
[
ξ
j
ξ
j
′
]
T
]
=
[
R
c
,
k
0
0
R
c
,
k
′
]
δ
k
,
j
\left[\begin{matrix} V_{c,k}\\ V_{c,k}' \end{matrix}\right]=\left[\begin{matrix} \Psi_{k/k-1}^{11}&\Psi_{k/k-1}^{12}\\ \Psi_{k/k-1}^{21}&\Psi_{k/k-1}^{22} \end{matrix}\right] \left[\begin{matrix}V_{c,k-1}\\ V_{c,k-1}' \end{matrix}\right]+ \left[\begin{matrix} \xi_{c,k}\\ \xi_{c,k}' \end{matrix}\right]\\ E[\left[\begin{matrix} \xi_{k}\\ \xi_{k}' \end{matrix}\right]]=0,E[\left[\begin{matrix} \xi_{k}\\ \xi_{k}' \end{matrix}\right]\left[\begin{matrix} \xi_{j}\\ \xi_{j}' \end{matrix}\right]^T]=\left[\begin{matrix} R_{c,k}&0\\ 0&R_{c,k}' \end{matrix}\right]\delta_{k,j}
[Vc,kVc,k′]=[Ψk/k−111Ψk/k−121Ψk/k−112Ψk/k−122][Vc,k−1Vc,k−1′]+[ξc,kξc,k′]E[[ξkξk′]]=0,E[[ξkξk′][ξjξj′]T]=[Rc,k00Rc,k′]δk,j
测量噪声表示为有限维状态带入状态空间
{
X
k
a
=
Φ
k
/
k
−
1
a
X
k
−
1
a
+
Γ
k
/
k
−
1
a
W
k
−
1
a
Z
k
=
H
k
a
X
k
a
+
V
k
s
t
.
{
E
[
W
w
,
k
a
]
=
0
,
E
[
W
w
,
k
a
W
w
,
j
a
T
]
=
Q
w
,
k
δ
k
j
=
d
i
a
g
(
Q
k
,
R
c
,
k
−
1
,
R
c
,
k
−
1
′
)
E
[
V
k
a
]
=
0
,
E
[
V
k
a
V
j
a
T
]
=
(
Θ
w
,
k
R
w
,
k
Θ
w
,
k
T
+
Θ
c
,
k
R
c
,
k
−
1
Θ
c
,
k
T
)
,
E
[
W
k
V
j
a
T
]
=
d
i
a
g
(
0
,
(
R
c
,
k
−
1
Θ
c
,
k
T
)
,
0
)
δ
k
j
R
≥
0
测
量
方
程
\begin{cases} X_k^a=\Phi_{k/k-1}^aX^a_{k-1}+\Gamma^a_{k/k-1}W^a_{k-1}&\\ Z_k=H^a_kX^a_k+V_k\\ \end{cases} \\ st. \\ \begin{cases} E[W^a_{w,k}]=0,E[W^a_{w,k}W_{w,j}^{aT}]=Q_{w,k}\delta_{kj}=diag(Q_{k},R_{c,k-1},R_{c,k-1}')\\ E[V_k^a]=0,E[V^a_kV_j^{aT}]=(\Theta_{w,k}R_{w,k}\Theta_{w,k}^T+\Theta_{c,k}R_{c,k-1}\Theta_{c,k}^T),E[W_kV_j^{aT}]=diag(0,(R_{c,k-1}\Theta_{c,k}^T),0)\delta_{kj}&R\geq 0&测量方程\\ \end{cases}
{Xka=Φk/k−1aXk−1a+Γk/k−1aWk−1aZk=HkaXka+Vkst.{E[Ww,ka]=0,E[Ww,kaWw,jaT]=Qw,kδkj=diag(Qk,Rc,k−1,Rc,k−1′)E[Vka]=0,E[VkaVjaT]=(Θw,kRw,kΘw,kT+Θc,kRc,k−1Θc,kT),E[WkVjaT]=diag(0,(Rc,k−1Θc,kT),0)δkjR≥0测量方程
X
k
a
=
[
X
k
V
c
,
k
−
1
V
c
,
k
−
1
′
]
,
Φ
k
/
k
−
1
a
=
[
Φ
k
/
k
−
1
Γ
c
,
k
−
1
0
0
Ψ
k
/
k
−
1
k
/
k
−
2
11
Ψ
k
/
k
−
1
k
/
k
−
2
12
0
Ψ
k
/
k
−
1
k
/
k
−
2
21
Ψ
k
/
k
−
1
k
/
k
−
2
22
]
Γ
k
a
=
[
Γ
k
0
0
0
I
0
0
0
I
]
,
W
k
a
=
[
W
w
,
k
ξ
k
−
1
ξ
k
−
1
′
]
,
H
k
a
=
[
H
k
ξ
k
−
1
ξ
k
−
1
′
]
,
V
k
a
=
Θ
w
,
k
V
w
.
k
+
Θ
c
,
k
ξ
k
−
1
X_k^a=\left[\begin{matrix} X_k\\ V_{c,k-1}\\ V_{c,k-1}' \end{matrix}\right],\Phi_{k/k-1}^a= \left[\begin{matrix} \Phi_{k/k-1}&\Gamma_{c,k-1}&0\\ 0&\Psi_{k/k-1 \ k/k-2}^{11}&\Psi_{k/k-1 \ k/k-2}^{12}\\ 0&\Psi_{k/k-1 \ k/k-2}^{21}&\Psi_{k/k-1 \ k/k-2}^{22}\\ \end{matrix}\right] \\ \Gamma_k^a=\left[\begin{matrix} \Gamma_{k}&0&0\\ 0&I&0\\ 0&0&I \end{matrix}\right],W_k^a= \left[\begin{matrix} W_{w,k}\\\xi_{k-1}\\\xi_{k-1}' \end{matrix}\right],H_k^a= \left[\begin{matrix} H_k&\xi_{k-1}&\xi_{k-1}'\\ \end{matrix}\right],V_k^a=\Theta_{w,k}V_{w.k}+\Theta_{c,k}\xi_{k-1}
Xka=⎣⎡XkVc,k−1Vc,k−1′⎦⎤,Φk/k−1a=⎣⎡Φk/k−100Γc,k−1Ψk/k−1 k/k−211Ψk/k−1 k/k−2210Ψk/k−1 k/k−212Ψk/k−1 k/k−222⎦⎤Γka=⎣⎡Γk000I000I⎦⎤,Wka=⎣⎡Ww,kξk−1ξk−1′⎦⎤,Hka=[Hkξk−1ξk−1′],Vka=Θw,kVw.k+Θc,kξk−1
测量求差法
如果测量方程中的有色误差
V
k
V_k
Vk可以表示为如下形式:
ξ
k
−
1
:
零
均
值
高
斯
误
差
,
E
[
ξ
k
]
=
0
,
E
[
ξ
k
ξ
j
T
]
=
R
ξ
,
k
δ
k
j
\xi_{k-1}:零均值高斯误差,E[\xi_k]=0,E[\xi_k\xi_j^T]=R_{\xi,k}\delta_{kj}
ξk−1:零均值高斯误差,E[ξk]=0,E[ξkξjT]=Rξ,kδkj
V
k
=
ψ
k
/
k
−
1
V
k
−
1
+
ξ
k
−
1
V_k=\psi_{k/k-1}V_{k-1}+\xi_{k-1}
Vk=ψk/k−1Vk−1+ξk−1
将相邻时刻的量求差并展开
Z
k
−
ψ
k
/
k
−
1
Z
k
/
k
−
1
=
[
H
k
(
Φ
k
/
k
−
1
X
k
−
1
+
Γ
k
/
k
−
1
W
k
−
1
)
+
(
ψ
k
/
k
−
1
V
k
−
1
+
ξ
k
−
1
)
]
−
ψ
k
/
k
−
1
(
H
k
−
1
X
k
−
1
+
V
k
−
1
)
=
(
H
k
Φ
k
/
k
−
1
−
ψ
k
/
k
−
1
H
k
−
1
)
X
k
−
1
+
(
H
k
Γ
k
/
k
−
1
W
k
−
1
+
ξ
k
−
1
)
Z_k-\psi_{k/k-1}Z_{k/k-1}=[H_k(\Phi_{k/k-1}X_{k-1}+\Gamma_{k/k-1}W_{k-1})+(\psi_{k/k-1}V_{k-1}+\xi_{k-1})]-\psi_{k/k-1}(H_{k-1}X_{k-1}+V_{k-1}) \\ =(H_k\Phi_{k/k-1}-\psi_{k/k-1}H_{k-1})X_{k-1}+(H_k\Gamma_{k/k-1}W_{k-1}+\xi_{k-1})
Zk−ψk/k−1Zk/k−1=[Hk(Φk/k−1Xk−1+Γk/k−1Wk−1)+(ψk/k−1Vk−1+ξk−1)]−ψk/k−1(Hk−1Xk−1+Vk−1)=(HkΦk/k−1−ψk/k−1Hk−1)Xk−1+(HkΓk/k−1Wk−1+ξk−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
−
1
δ
k
j
E
[
V
k
∗
]
=
0
,
E
[
V
k
∗
(
W
j
∗
)
T
]
=
(
H
k
Γ
k
−
1
Q
k
−
1
Γ
k
−
1
T
H
k
T
+
R
ξ
,
k
−
1
)
δ
k
j
E
[
W
k
∗
W
j
∗
T
]
=
Q
k
−
1
Γ
k
−
1
T
H
k
T
δ
k
j
\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_k^*(W_j^*)^T]=Q_{k-1}\delta_{kj}\\ E[V_k^*]=0,E[V_k^*(W_j^*)^T]=(H_k\Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^TH_k^T+R_{\xi,k-1})\delta_{kj}\\ E[W_k^*W_{j}^{*T}]=Q_{k-1}\Gamma_{k-1}^TH_k^T\delta_{kj}\\ \end{cases} \\
{Xk∗=Φk/k−1∗Xk−1∗+Γk/k−1∗Wk−1∗Zk∗=Hk∗Xk∗+Vk∗st:⎩⎪⎨⎪⎧E[Wk∗]=0,E[Wk∗(Wj∗)T]=Qk−1δkjE[Vk∗]=0,E[Vk∗(Wj∗)T]=(HkΓk−1Qk−1Γk−1THkT+Rξ,k−1)δkjE[Wk∗Wj∗T]=Qk−1Γk−1THkTδkj
Z
k
∗
=
Z
k
−
ψ
k
/
k
−
1
Z
k
/
k
−
1
,
H
k
∗
=
H
k
Φ
k
/
k
−
1
−
ψ
k
/
k
−
1
H
k
−
1
,
V
k
∗
=
H
k
Γ
k
/
k
−
1
W
k
−
1
+
ξ
k
−
1
X
k
∗
=
X
k
−
1
,
Φ
k
/
k
−
1
∗
=
Φ
k
−
1
/
k
−
2
,
Γ
k
∗
=
Γ
k
−
1
,
W
k
∗
=
W
k
−
1
Z_k^*=Z_k-\psi_{k/k-1}Z_{k/k-1},H^*_k=H_k\Phi_{k/k-1}-\psi_{k/k-1}H_{k-1},V^*_k=H_k\Gamma_{k/k-1}W_{k-1}+\xi_{k-1}\\ X^*_{k}=X_{k-1},\Phi^*_{k/k-1}=\Phi_{k-1/k-2},\Gamma_k^*=\Gamma_{k-1},W_k^*=W_{k-1}
Zk∗=Zk−ψk/k−1Zk/k−1,Hk∗=HkΦk/k−1−ψk/k−1Hk−1,Vk∗=HkΓk/k−1Wk−1+ξk−1Xk∗=Xk−1,Φk/k−1∗=Φk−1/k−2,Γk∗=Γk−1,Wk∗=Wk−1