问题描述
存在N个传感器进行观测，观测数据有一定概率丢失
简而言之，其中
γ
\gamma
γ是一个随机数（非0即1），0代表数据丢失，1代表数据存在。其他的多传感器多速率表示与上篇文章一致。
下面同样对多速率模型进行改写转化为单模型。
其中
算法推导
对于多速率多传感器状态转移方程重构参数的推导在上一篇文章中已经详尽的进行了叙述，这里只对观测方程也就是不同的部分进行推导讨论。
为了便于推导这个对状态转移方程进行简化，控制项设为0，噪声增益矩阵
Γ
\Gamma
Γ设为单位阵，即为
x
(
k
+
1
)
=
A
(
k
)
x
(
k
)
+
w
(
k
)
\ x(k+1)=A(k)x(k)+w(k)
x(k+1)=A(k)x(k)+w(k)，可以得到
同理可以写出：
此时将
x
(
l
i
(
k
i
−
1
)
+
1
)
\ x(l_i(k_i-1)+1)
x(li(ki−1)+1)移到左侧可以得到
那么根据观测方程我们可以得到：
在将上面推导的状态转移方程代入观测方程可以得到：
这里问什么要把
x
(
l
i
(
k
i
−
1
)
+
1
)
\ x(l_i(k_i-1)+1)
x(li(ki−1)+1)代入，个人认为：因为传感器
i
(
i
>
1
)
\ i(i>1)
i(i>1)不是在最细尺度上采样，那么对于没有采样数据的时刻点如果想要充分的利用观测数据可以用
x
(
l
i
(
k
i
−
1
)
+
1
)
\ x(l_i(k_i-1)+1)
x(li(ki−1)+1)与
x
(
l
i
(
k
i
−
1
)
+
j
)
\ x(l_i(k_i-1)+j)
x(li(ki−1)+j)的对应关系进行代换，将
x
(
l
i
(
k
i
−
1
)
+
j
)
\ x(l_i(k_i-1)+j)
x(li(ki−1)+j)还原到
x
(
l
i
(
k
i
−
1
)
+
1
)
\ x(l_i(k_i-1)+1)
x(li(ki−1)+1)上利用
z
(
l
i
(
k
i
−
1
)
+
1
)
\ z(l_i(k_i-1)+1)
z(li(ki−1)+1)进行修正（通过
C
i
\ C_i
Ci可以看出）。
这里可以看出重构的观测方程参数，等式右侧第一项为
γ
(
i
,
k
i
)
C
i
x
(
l
i
(
k
i
−
1
)
+
j
)
\gamma(i,k_i)C_ix(l_i(k_i-1)+j)
γ(i,ki)Cix(li(ki−1)+j),第二项与第三项的和为
v
i
(
k
)
\ v_i(k)
vi(k)
根据这两项可以推出问题描述中给出的重构的参数
C
i
,
R
i
\ C_i,R_i
Ci,Ri
如果不对状态转移矩阵进行简化那么重构的参数是怎么样的？
首先
Γ
,
A
(
k
)
,
G
i
(
k
)
\Gamma,A(k),G_i(k)
Γ,A(k),Gi(k)与上篇中重构得到的是一样的。
其次，
C
i
\ C_i
Ci也不会发生变化(因为在算式的推导中
C
i
\ C_i
Ci都是从等式右侧第一项中取)，变化的是
v
i
\ v_i
vi。（不确定理解的对不对）
下面进行推导：
x
(
l
i
(
k
i
−
1
)
+
2
)
=
A
(
l
i
(
k
i
−
1
)
+
1
)
x
(
l
i
(
k
i
−
1
)
+
1
)
+
G
(
l
i
(
k
i
−
1
)
+
1
)
u
(
l
i
(
k
i
−
1
)
+
1
)
+
Γ
(
l
i
(
k
i
−
1
)
+
1
)
w
(
l
i
(
k
i
−
1
)
+
1
)
\ x(l_i(k_i-1)+2)=A(l_i(k_i-1)+1)x(l_i(k_i-1)+1)+G(l_i(k_i-1)+1)u(l_i(k_i-1)+1)+\Gamma(l_i(k_i-1)+1)w(l_i(k_i-1)+1)
x(li(ki−1)+2)=A(li(ki−1)+1)x(li(ki−1)+1)+G(li(ki−1)+1)u(li(ki−1)+1)+Γ(li(ki−1)+1)w(li(ki−1)+1)
对
j
=
3
,
4
,
.
.
.
,
l
i
\ j=3,4,...,l_i
j=3,4,...,li有：
x
(
l
i
(
k
i
−
1
)
+
j
)
=
A
(
l
i
(
k
i
−
1
)
+
j
−
1
)
x
(
l
i
(
k
i
−
1
)
+
j
−
1
)
+
G
(
l
i
(
k
i
−
1
)
+
j
−
1
)
u
(
l
i
(
k
i
−
1
)
+
j
−
1
)
+
Γ
(
l
i
(
k
i
−
1
)
+
j
−
1
)
w
(
l
i
(
k
i
−
1
)
+
j
−
1
)
=
∏
l
=
j
−
1
1
A
(
l
i
(
k
i
−
1
)
+
l
)
x
(
l
i
(
k
i
−
1
)
+
1
)
+
∑
p
=
1
j
−
2
∏
l
=
j
−
1
p
+
1
A
(
l
i
(
k
i
−
1
)
+
l
)
G
(
l
i
(
k
i
−
1
)
+
p
)
u
(
l
i
(
k
i
−
1
)
+
p
)
+
∑
p
=
1
j
−
2
∏
l
=
j
−
1
p
+
1
A
(
l
i
(
k
i
−
1
)
+
l
)
Γ
(
l
i
(
k
i
−
1
)
+
p
)
w
(
l
i
(
k
i
−
1
)
+
p
)
+
G
(
l
i
(
k
i
−
1
)
+
j
−
1
)
u
(
l
i
(
k
i
−
1
)
+
j
−
1
)
+
Γ
(
l
i
(
k
i
−
1
)
+
j
−
1
)
w
(
l
i
(
k
i
−
1
)
+
j
−
1
)
\ x(l_i(k_i-1)+j)=A(l_i(k_i-1)+j-1)x(l_i(k_i-1)+j-1)+G(l_i(k_i-1)+j-1)u(l_i(k_i-1)+j-1)+\Gamma(l_i(k_i-1)+j-1)w(l_i(k_i-1)+j-1)=\prod_{l=j-1}^1A(l_i(k_i-1)+l)x(l_i(k_i-1)+1)+\sum^{j-2}_{p=1}\prod_{l=j-1}^{p+1}A(l_i(k_i-1)+l)G(l_i(k_i-1)+p)u(l_i(k_i-1)+p)+\sum^{j-2}_{p=1}\prod_{l=j-1}^{p+1}A(l_i(k_i-1)+l)\Gamma(l_i(k_i-1)+p)w(l_i(k_i-1)+p)+G(l_i(k_i-1)+j-1)u(l_i(k_i-1)+j-1)+\Gamma(l_i(k_i-1)+j-1)w(l_i(k_i-1)+j-1)
x(li(ki−1)+j)=A(li(ki−1)+j−1)x(li(ki−1)+j−1)+G(li(ki−1)+j−1)u(li(ki−1)+j−1)+Γ(li(ki−1)+j−1)w(li(ki−1)+j−1)=∏l=j−11A(li(ki−1)+l)x(li(ki−1)+1)+∑p=1j−2∏l=j−1p+1A(li(ki−1)+l)G(li(ki−1)+p)u(li(ki−1)+p)+∑p=1j−2∏l=j−1p+1A(li(ki−1)+l)Γ(li(ki−1)+p)w(li(ki−1)+p)+G(li(ki−1)+j−1)u(li(ki−1)+j−1)+Γ(li(ki−1)+j−1)w(li(ki−1)+j−1)
将
x
(
l
i
(
k
i
−
1
)
+
1
)
x(l_i(k_i-1)+1)
x(li(ki−1)+1)变换到左侧可以得到：
x
(
l
i
(
k
i
−
1
)
+
1
)
=
∏
l
=
1
j
−
1
A
−
1
x
(
l
i
(
k
i
−
1
)
+
j
)
−
∑
p
=
1
j
−
1
∏
l
=
1
p
A
−
1
x
(
l
i
(
k
i
−
1
)
+
l
)
G
(
l
i
(
k
i
−
1
)
+
p
)
u
(
l
i
(
k
i
−
1
)
+
p
)
−
∑
p
=
1
j
−
1
∏
l
=
1
p
A
−
1
x
(
l
i
(
k
i
−
1
)
+
l
)
Γ
(
l
i
(
k
i
−
1
)
+
p
)
w
(
l
i
(
k
i
−
1
)
+
p
)
x(l_i(k_i-1)+1)=\prod^{j-1}_{l=1}A^{-1}x(l_i(k_i-1)+j)-\sum^{j-1}_{p=1}\prod^p_{l=1}A^{-1}x(l_i(k_i-1)+l)G(l_i(k_i-1)+p)u(l_i(k_i-1)+p)-\sum^{j-1}_{p=1}\prod^p_{l=1}A^{-1}x(l_i(k_i-1)+l)\Gamma(l_i(k_i-1)+p)w(l_i(k_i-1)+p)
x(li(ki−1)+1)=∏l=1j−1A−1x(li(ki−1)+j)−∑p=1j−1∏l=1pA−1x(li(ki−1)+l)G(li(ki−1)+p)u(li(ki−1)+p)−∑p=1j−1∏l=1pA−1x(li(ki−1)+l)Γ(li(ki−1)+p)w(li(ki−1)+p)
将上式子代入观测方程，可以得出其中的
v
i
(
k
)
v_i(k)
vi(k)这里就不给出代入后的式子了，直接写出得到的
v
i
(
k
)
v_i(k)
vi(k)。
v
i
(
k
)
=
{
v
(
i
,
k
i
)
k
=
l
i
(
k
i
−
1
)
+
1
v
(
i
,
k
i
)
−
γ
(
i
,
k
i
)
C
(
i
,
k
i
)
[
∑
p
=
1
j
−
1
∏
l
=
1
p
A
−
1
x
(
l
i
(
k
i
−
1
)
+
l
)
G
(
l
i
(
k
i
−
1
)
+
p
)
u
(
l
i
(
k
i
−
1
)
+
p
)
+
∑
p
=
1
j
−
1
∏
l
=
1
p
A
−
1
x
(
l
i
(
k
i
−
1
)
+
l
)
Γ
(
l
i
(
k
i
−
1
)
+
p
)
w
(
l
i
(
k
i
−
1
)
+
p
)
]
k
=
l
i
(
k
i
−
1
)
+
j
,
j
=
2
,
3
,
.
.
.
,
l
i
v_i(k)=\left\{ \begin{array}{lcl} v(i,k_i) & & {k=l_i(k_i-1)+1}\\ v(i,k_i) -\gamma(i,k_i)C(i,k_i)[\sum^{j-1}_{p=1}\prod^p_{l=1}A^{-1}x(l_i(k_i-1)+l)G(l_i(k_i-1)+p)u(l_i(k_i-1)+p)+\sum^{j-1}_{p=1}\prod^p_{l=1}A^{-1}x(l_i(k_i-1)+l)\Gamma(l_i(k_i-1)+p)w(l_i(k_i-1)+p)] & & {k=l_i(k_i-1)+j,j=2,3,...,l_i} \end{array} \right.
vi(k)={v(i,ki)v(i,ki)−γ(i,ki)C(i,ki)[∑p=1j−1∏l=1pA−1x(li(ki−1)+l)G(li(ki−1)+p)u(li(ki−1)+p)+∑p=1j−1∏l=1pA−1x(li(ki−1)+l)Γ(li(ki−1)+p)w(li(ki−1)+p)]k=li(ki−1)+1k=li(ki−1)+j,j=2,3,...,li
R
i
(
k
)
=
E
[
v
i
(
k
)
v
i
T
(
k
)
]
=
E
[
{
v
(
i
,
k
i
)
−
γ
(
i
,
k
i
)
C
(
i
,
k
i
)
[
∑
p
=
1
j
−
1
∏
l
=
1
p
A
−
1
x
(
l
i
(
k
i
−
1
)
+
l
)
G
(
l
i
(
k
i
−
1
)
+
p
)
u
(
l
i
(
k
i
−
1
)
+
p
)
+
∑
p
=
1
j
−
1
∏
l
=
1
p
A
−
1
x
(
l
i
(
k
i
−
1
)
+
l
)
Γ
(
l
i
(
k
i
−
1
)
+
p
)
w
(
l
i
(
k
i
−
1
)
+
p
)
]
}
⋅
{
v
(
i
,
k
i
)
−
γ
(
i
,
k
i
)
C
(
i
,
k
i
)
[
∑
p
=
1
j
−
1
∏
l
=
1
p
A
−
1
x
(
l
i
(
k
i
−
1
)
+
l
)
G
(
l
i
(
k
i
−
1
)
+
p
)
u
(
l
i
(
k
i
−
1
)
+
p
)
+
∑
p
=
1
j
−
1
∏
l
=
1
p
A
−
1
x
(
l
i
(
k
i
−
1
)
+
l
)
Γ
(
l
i
(
k
i
−
1
)
+
p
)
w
(
l
i
(
k
i
−
1
)
+
p
)
]
}
T
]
=
γ
‾
i
C
(
i
,
k
i
)
[
∑
p
=
0
j
−
1
(
∏
l
=
1
p
A
−
1
(
l
i
(
k
i
)
−
1
)
+
l
)
Γ
(
l
i
(
k
i
−
1
)
+
p
)
Q
(
l
i
(
k
i
−
1
)
+
p
)
Γ
T
(
l
i
(
k
i
−
1
)
+
p
)
∏
l
=
p
1
A
−
T
(
l
i
(
k
i
)
−
1
)
+
l
)
+
∑
p
=
0
j
−
1
(
∏
l
=
1
p
A
−
1
(
l
i
(
k
i
)
−
1
)
+
l
)
G
(
l
i
(
k
i
−
1
)
+
p
)
u
(
l
i
(
k
i
−
1
)
+
p
)
u
T
(
l
i
(
k
i
−
1
)
+
p
)
G
T
(
l
i
(
k
i
−
1
)
+
p
)
∏
l
=
p
1
A
−
T
(
l
i
(
k
i
)
−
1
)
+
l
)
]
C
T
(
i
,
k
i
)
+
R
i
(
i
,
k
i
)
R_i(k)=E[v_i(k)v_i^T(k)]=E[\{v(i,k_i) -\gamma(i,k_i)C(i,k_i)[\sum^{j-1}_{p=1}\prod^p_{l=1}A^{-1}x(l_i(k_i-1)+l)G(l_i(k_i-1)+p)u(l_i(k_i-1)+p)+\sum^{j-1}_{p=1}\prod^p_{l=1}A^{-1}x(l_i(k_i-1)+l)\Gamma(l_i(k_i-1)+p)w(l_i(k_i-1)+p)]\}·\{v(i,k_i) -\gamma(i,k_i)C(i,k_i)[\sum^{j-1}_{p=1}\prod^p_{l=1}A^{-1}x(l_i(k_i-1)+l)G(l_i(k_i-1)+p)u(l_i(k_i-1)+p)+\sum^{j-1}_{p=1}\prod^p_{l=1}A^{-1}x(l_i(k_i-1)+l)\Gamma(l_i(k_i-1)+p)w(l_i(k_i-1)+p)]\}^T]=\overline{\gamma}_iC(i,k_i)[\sum^{j-1}_{p=0}(\prod^p_{l=1}A^{-1}(l_i(k_i)-1)+l)\Gamma(l_i(k_i-1)+p)Q(l_i(k_i-1)+p)\Gamma^T(l_i(k_i-1)+p)\prod^1_{l=p}A^{-T}(l_i(k_i)-1)+l)+\sum^{j-1}_{p=0}(\prod^p_{l=1}A^{-1}(l_i(k_i)-1)+l)G(l_i(k_i-1)+p)u(l_i(k_i-1)+p)u^T(l_i(k_i-1)+p)G^T(l_i(k_i-1)+p)\prod^1_{l=p}A^{-T}(l_i(k_i)-1)+l)]C^T(i,k_i)+R_i(i,k_i)
Ri(k)=E[vi(k)viT(k)]=E[{v(i,ki)−γ(i,ki)C(i,ki)[∑p=1j−1∏l=1pA−1x(li(ki−1)+l)G(li(ki−1)+p)u(li(ki−1)+p)+∑p=1j−1∏l=1pA−1x(li(ki−1)+l)Γ(li(ki−1)+p)w(li(ki−1)+p)]}⋅{v(i,ki)−γ(i,ki)C(i,ki)[∑p=1j−1∏l=1pA−1x(li(ki−1)+l)G(li(ki−1)+p)u(li(ki−1)+p)+∑p=1j−1∏l=1pA−1x(li(ki−1)+l)Γ(li(ki−1)+p)w(li(ki−1)+p)]}T]=γiC(i,ki)[∑p=0j−1(∏l=1pA−1(li(ki)−1)+l)Γ(li(ki−1)+p)Q(li(ki−1)+p)ΓT(li(ki−1)+p)∏l=p1A−T(li(ki)−1)+l)+∑p=0j−1(∏l=1pA−1(li(ki)−1)+l)G(li(ki−1)+p)u(li(ki−1)+p)uT(li(ki−1)+p)GT(li(ki−1)+p)∏l=p1A−T(li(ki)−1)+l)]CT(i,ki)+Ri(i,ki)
因为其中的噪声
w
(
i
,
k
i
)
,
v
(
i
,
k
i
)
w(i,k_i),v(i,k_i)
w(i,ki),v(i,ki)均为均值为0的高斯白噪声且具有统计无关性，这是上述推导中需要注意的。
融合算法
这里的
M
(
k
)
\ M(k)
M(k)集合实际上是丢失数据时刻的集合，那么如何理解上述集合的数学描述，个人理解：数据丢失即无法对预测得到的数据起到滤波修正的作用，从集合中可以看到
E
{
[
z
i
(
k
)
−
C
i
(
k
)
x
^
(
k
∣
k
−
1
)
]
[
z
i
(
k
)
−
C
i
(
k
)
x
^
(
k
∣
k
−
1
)
]
T
}
\ E\{[z_i(k)-C_i(k)\hat{x}(k|k-1)][z_i(k)-C_i(k)\hat{x}(k|k-1)]^T\}
E{[zi(k)−Ci(k)x^(k∣k−1)][zi(k)−Ci(k)x^(k∣k−1)]T}这个均值所代表的意义是观测数据与预测值的差值的噪声矩阵，因为从概率意义上是将观测值与预测值的均值视为真实值的，不过实际得到的数据都是样本是服从概率分布的，那么从概率上将上述那个式子可以视为均值为0的一个概率分布的方差。那么如果这个方差无限接近于传感器的观测噪声矩阵
R
i
\ R_i
Ri，那么此时就说明那么就说明观测值与预测值之间偏差的修正作用被传感器的观测噪声所掩盖，失去了滤波修正的作用，体现了数据丢失。
仿真实例
设定
u
(
k
)
=
0
,
Γ
(
k
)
=
I
u(k)=0,\Gamma(k)=I
u(k)=0,Γ(k)=ISINS,GPS,SM分别为传感器1，2，3。
观测矩阵：
这里程序对第二种情况进行了仿真。针对本次仿真的设定可以采用简化模型推导得到的
R
i
(
k
)
R_i(k)
Ri(k)
%%%===========随机丢包情况下多速率传感器鲁棒融合设计========%%%
clc;
clear all;
close all;
%% 数据生成
%设某飞机在某固定高度以近似匀加速飞行(假设方向为东北方向)
%状态矩阵x为9x1 状态分别为东向位置/东向速度/东向加速度/北向位置/北向速度/北向加速度/天向位置/天向速度/天向加速度
%x=[xe ve ae xn vn an xu vu au]';
%生成实际飞行数据飞行加速度为5m/s^2
N=500;%时间跨度
n1=1; %SINS采样间隔
n2=2; %GPS采样间隔
n3=10; %SM采样间隔
M=lcm(n2,n3);%采样间隔的最小公倍数 即为融合周期
M1=M/n1;
M2=M/n2;
M3=M/n3;
t=1:1:N ; %时间序列
X0=[0 0 0 0 300 0 800 0 0 ];%初始状态
P0=diag([100,16,1,100,16,1,100,16,1]);
%实际数据
xe=0.5*5*0.707*t.^2;
ve=5*0.707*t;
ae=5*0.707*ones(1,N);
xn=300*t+0.5*5*0.707*t.^2;
vn=300+5*0.707*t;
an=5*0.707*ones(1,N);
xu=800*ones(1,N);
vu=zeros(1,N);
au=zeros(1,N);
%观测数据的生成
%1.加噪声 由于不同传感器对位置/速度以及加速度的量测噪声不尽相同，所以逐个加噪声。
%SINS东北天向位置/速度/加速度 惯导
SINS_xe=xe+randn(1,N)*sqrt(90000);
SINS_ve=ve+randn(1,N)*sqrt(0.01);
SINS_ae=ae+randn(1,N)*sqrt(10^-8);
SINS_xn=xn+randn(1,N)*sqrt(90000);
SINS_vn=vn+randn(1,N)*sqrt(0.01);
SINS_an=an+randn(1,N)*sqrt(10^-8);
SINS_xu=xu+randn(1,N)*sqrt(90000);
SINS_vu=vu+randn(1,N)*sqrt(0.01);
SINS_au=au+randn(1,N)*sqrt(10^-8);
%GPS东北天向位置/速度
GPS_xe=xe+randn(1,N)*sqrt(2500);
GPS_ve=ve+randn(1,N)*sqrt(0.01);
GPS_xn=xn+randn(1,N)*sqrt(2500);
GPS_vn=vn+randn(1,N)*sqrt(0.01);
GPS_xu=xu+randn(1,N)*sqrt(2500);
GPS_vu=vu+randn(1,N)*sqrt(0.01);
%SM 景象匹配 东北向位置数据
SM_xe=xe+randn(1,N)*sqrt(100);
SM_xn=xn+randn(1,N)*sqrt(100);
%观测数据
%SINS
SINS_z=cell(9,1);
SINS_z(1,1)={SINS_xe};
SINS_z(2,1)={SINS_ve};
SINS_z(3,1)={SINS_ae};
SINS_z(4,1)={SINS_xn};
SINS_z(5,1)={SINS_vn};
SINS_z(6,1)={SINS_an};
SINS_z(7,1)={SINS_xu};
SINS_z(8,1)={SINS_vu};
SINS_z(9,1)={SINS_au};
SINS_z=cell2mat(SINS_z);
%GPS
GPS_z=cell(6,1);
GPS_z(1,1)={GPS_xe};
GPS_z(2,1)={GPS_ve};
GPS_z(3,1)={GPS_xn};
GPS_z(4,1)={GPS_vn};
GPS_z(5,1)={GPS_xu};
GPS_z(6,1)={GPS_vu};
GPS_z=cell2mat(GPS_z);
%SM
SM_z=cell(2,1);
SM_z(1,1)={SM_xe};
SM_z(2,1)={SM_xn};
SM_z=cell2mat(SM_z);
%% 转移矩阵/观测矩阵/噪声矩阵/ 此处可以使用稀疏矩阵建立以节约内存 A=sparse(r,c,S)
%转移矩阵
A=[1 1 0.5 0 0 0 0 0 0
0 1 1 0 0 0 0 0 0
0 0 1 0 0 0 0 0 0
0 0 0 1 1 0.5 0 0 0
0 0 0 0 1 1 0 0 0
0 0 0 0 0 1 0 0 0
0 0 0 0 0 0 1 1 0.5
0 0 0 0 0 0 0 1 1
0 0 0 0 0 0 0 0 1];
%观测矩阵
C1=eye(9);
C2=[1 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0
0 0 0 1 0 0 0 0 0
0 0 0 0 1 0 0 0 0
0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 0 1 0];
C3=[1 0 0 0 0 0 0 0 0
0 0 0 1 0 0 0 0 0];
%噪声矩阵
R1=diag([90000 0.01 10^-8 90000 0.01 10^-8 90000 0.01 10^-8]);
R2=diag([2500 0.01 2500 0.01 2500 0.01]);
R3=diag([100 100]);
Q=(A^2)*5;
%% 滤波
%假设三个传感器同步得到数据的时刻为i=1,i=11……501
X=cell(1,501);
X(1)={X0'};
for i=1:N
if rem(i-1,n2)==0
if rem(i-1,n3)==0
%此种情况三种传感器均得到数据
%随机丢失数生成
shadow1=rand(1,1);
shadow2=rand(1,1);
shadow3=rand(1,1);
%预测
temp=A*cell2mat(X(i));
P1=A*P0*A'+Q;
%数据丢失判断
if shadow1<0.0001 %丢失几率为10^-4
K1=zeros(9,9);
else
K1=P1*C1'/(C1*P1*C1'+R1);
end
if shadow2<0.0001 %丢失几率为10^-4
K2=zeros(9,6);
else
K2=P1*C2'/(C2*P1*C2'+R2);
end
if shadow3<0.01 %丢失几率为10^-2
K3=zeros(9,2);
else
K3=P1*C3'/(C3*P1*C3'+R3);
end
%K值得出完毕
%进行估计
x1=temp+K1*(SINS_z(:,i)-C1*temp);
x2=temp+K2*(GPS_z(:,i)-C2*temp);
x3=temp+K3*(SM_z(:,i)-C3*temp);
p1=(eye(9,9)-K1*C1)*P1;
p2=(eye(9,9)-K2*C2)*P1;
p3=(eye(9,9)-K3*C3)*P1;
p_1=inv(p1);
p_2=inv(p2);
p_3=inv(p3);
%进行融合
P0=inv(p_1+p_2+p_3);
X(i+1)={P0*(p_1*x1+p_2*x2+p_3*x3)};
else
%此种情况只有SINS与GPS得到数据
%随机丢失数生成
shadow1=rand(1,1);
shadow2=rand(1,1);
%预测
temp=A*cell2mat(X(i));
P1=A*P0*A'+Q;
j3=rem(i-1,n3);
%数据丢失判断
if shadow1<0.0001 %丢失几率为10^-4
K1=zeros(9,9);
else
K1=P1*C1'/(C1*P1*C1'+R1);
end
if shadow2<0.0001 %丢失几率为10^-4
K2=zeros(9,6);
else
K2=P1*C2'/(C2*P1*C2'+R2);
end
%得到噪声矩阵
%R3
R_3=zeros(9,9);
for n=1:j3
R_3=(inv(A)^n)*Q*(inv(A')^n)+R_3;
end
R_3=0.99*C3*(inv(A)^(j3))*R_3*(C3*(inv(A)^(j3)))'+R3;
%得到增益矩阵
K3=P1*(C3*(inv(A)^(j3)))'/(C3*inv(A)^(j3)*P1*(C3*(inv(A)^(j3)))'+R_3);
%估计
x1=temp+K1*(SINS_z(:,i)-C1*temp);
x2=temp+K2*(GPS_z(:,i)-C2*temp);
x3=temp+K3*(SM_z(:,i-j3)-C3*(inv(A)^(j3))*temp);
p1=(eye(9,9)-K1*C1)*P1;
p2=(eye(9,9)-K2*C2)*P1;
p3=(eye(9,9)-K3*C3*inv(A)^(j3))*P1;
p_1=inv(p1);
p_2=inv(p2);
p_2=inv(p3);
%进行融合
P0=inv(p_1+p_2+p_3);
X(i+1)={P0*(p_1*x1+p_2*x2+p_3*x3)};
end
else
if rem(i-1,n3)==0
%此种情况下得到SINS与SM传感器信息
%随机丢失数生成
shadow1=rand(1,1);
shadow3=rand(1,1);
%预测
temp=A*cell2mat(X(i));
P1=A*P0*A'+Q;
j2=rem(i-1,n2);
%数据丢失判断
if shadow1<0.0001 %丢失几率为10^-4
K1=zeros(9,9);
else
K1=P1*C1'/(C1*P1*C1'+R1);
end
if shadow3<0.01 %丢失几率为10^-2
K3=zeros(9,2);
else
K3=P1*C3'/(C3*P1*C3'+R3);
end
%得到噪声矩阵
%R2
R_2=zeros(9,9);
for n=1:j2
R_2=(inv(A)^n)*Q*(inv(A')^n)+R_2;
end
R_2=0.9999*C2*(inv(A)^(j2))*R_2*(C2*(inv(A)^(j2)))'+R2;
%得到增益矩阵
K2=P1*(C2*(inv(A)^(j2)))'/(C2*inv(A)^(j2)*P1*(C2*(inv(A)^(j2)))'+R_2);
%估计
x1=temp+K1*(SINS_z(:,i)-C1*temp);
x2=temp+K2*(GPS_z(:,i-j2)-C2*(inv(A)^(j2))*temp);
x3=temp+K3*(SM_z(:,i)-C3*temp);
p1=(eye(9,9)-K1*C1)*P1;
p2=(eye(9,9)-K2*C2*inv(A)^(j2))*P1;
p3=(eye(9,9)-K3*C3)*P1;
p_1=inv(p1);
p_2=inv(p2);
p_2=inv(p3);
%进行融合
P0=inv(p_1+p_2+p_3);
X(i+1)={P0*(p_1*x1+p_2*x2+p_3*x3)};
else
%此种情况下，只得到SINS数据
%随机丢失数生成
shadow1=rand(1,1);
%预测
temp=A*cell2mat(X(i));
P1=A*P0*A'+Q;
j2=rem(i-1,n2);
j3=rem(i-1,n3);
%数据丢失判断
if shadow1<0.0001 %丢失几率为10^-4
K1=zeros(9,9);
else
K1=P1*C1'/(C1*P1*C1'+R1);
end
%得到噪声矩阵
%R2
R_2=zeros(9,9);
for n=1:j2
R_2=(inv(A)^n)*Q*(inv(A')^n)+R_2;
end
R_2=0.9999*C2*(inv(A)^(j2))*R_2*(C2*(inv(A)^(j2)))'+R2;
%R3
R_3=zeros(9,9);
for n=1:j3
R_3=(inv(A)^n)*Q*(inv(A')^n)+R_3;
end
R_3=0.99*C3*(inv(A)^(j3))*R_3*(C3*(inv(A)^(j3)))'+R3;
%得到增益矩阵
K2=P1*(C2*(inv(A)^(j2)))'/(C2*inv(A)^(j2)*P1*(C2*(inv(A)^(j2)))'+R_2);
K3=P1*(C3*(inv(A)^(j3)))'/(C3*inv(A)^(j3)*P1*(C3*(inv(A)^(j3)))'+R_3);
%估计
x1=temp+K1*(SINS_z(:,i)-C1*temp);
x2=temp+K2*(GPS_z(:,i-j2)-C2*(inv(A)^(j2))*temp);
x3=temp+K3*(SM_z(:,i-j3)-C3*(inv(A)^(j3))*temp);
p1=(eye(9,9)-K1*C1)*P1;
p2=(eye(9,9)-K2*C2*inv(A)^(j2))*P1;
p3=(eye(9,9)-K3*C3*inv(A)^(j3))*P1;
p_1=inv(p1);
p_2=inv(p2);
p_2=inv(p3);
%进行融合
P0=inv(p_1+p_2+p_3);
X(i+1)={P0*(p_1*x1+p_2*x2+p_3*x3)};
end
end
end
%% 图像绘制
l=1:1:500;
X=cell2mat(X);
X=X(1:9,2:501);
%实际与估计值比较
subplot(3,1,1)
plot(l,X(1,:),'b',l,xe,'r');
title('东向位置');
legend('估计值','实际值');
xlabel('t');
ylabel('s');
subplot(3,1,2)
plot(l,X(2,:),'b',l,ve,'r');
title('东向速度');
legend('估计值','实际值');
xlabel('t');
ylabel('v');
subplot(3,1,3)
plot(l,X(3,:),'b',l,ae,'r');
title('东向加速度');
legend('估计值','实际值');
xlabel('t');
ylabel('a');
figure(2)
subplot(3,1,1)
plot(l,X(4,:),'b',l,xn,'r');
title('北向位置');
legend('估计值','实际值');
xlabel('t');
ylabel('s');
subplot(3,1,2)
plot(l,X(5,:),'b',l,vn,'r');
title('北向速度');
legend('估计值','实际值');
xlabel('t');
ylabel('v');
subplot(3,1,3)
plot(l,X(6,:),'b',l,an,'r');
title('北向加速度');
legend('估计值','实际值');
xlabel('t');
ylabel('a');
figure(3)
subplot(3,1,1)
plot(l,X(7,:),'b',l,xu,'r');
title('天向位置');
legend('估计值','实际值');
xlabel('t');
ylabel('s');
subplot(3,1,2)
plot(l,X(8,:),'b',l,vu,'r');
title('天向速度');
legend('估计值','实际值');
xlabel('t');
ylabel('v');
subplot(3,1,3)
plot(l,X(9,:),'b',l,au,'r');
title('天向加速度');
legend('估计值','实际值');
xlabel('t');
ylabel('a');
%误差
figure(4)
subplot(3,1,1)
plot(l,X(1,:)-xe,'r');
title('东向位置绝对误差');
xlabel('t');
ylabel('δ');
subplot(3,1,2)
plot(l,X(2,:)-ve,'r');
title('东向速度绝对误差');
xlabel('t');
ylabel('δ');
subplot(3,1,3)
plot(l,X(3,:)-ae,'r');
title('东向加速度绝对误差');
xlabel('t');
ylabel('δ');
figure(5)
subplot(3,1,1)
plot(l,X(4,:)-xn,'r');
title('北向位置绝对误差');
xlabel('t');
ylabel('δ');
subplot(3,1,2)
plot(l,X(5,:)-vn,'r');
title('北向速度绝对误差');
xlabel('t');
ylabel('δ');
subplot(3,1,3)
plot(l,X(6,:)-an,'r');
title('北向加速度绝对误差');
xlabel('t');
ylabel('δ');
figure(6)
subplot(3,1,1)
plot(l,X(7,:)-xu,'r');
title('天向位置绝对误差');
xlabel('t');
ylabel('δ');
subplot(3,1,2)
plot(l,X(8,:)-vu,'r');
title('天向速度绝对误差');
xlabel('t');
ylabel('δ');
subplot(3,1,3)
plot(l,X(9,:)-au,'r');
title('天向加速度绝对误差');
xlabel('t');
ylabel('δ');
Caution
文中程序中的轨迹数据没用加入系统噪声，具体的系统噪声生成形式应该是递推形式：
类似下述形式，其实Kalman滤波器能滤除的噪声仅限于观测噪声，不加系统噪声对实际的结果造成的影响不大，如果严格而言的话会存在系统状态误差协方差的估计与实际之间的偏差，导致系统更偏向于依赖观测值，其实此时系统递推值应该是更值得依赖的，因为不存在系统噪声，这是其中存在的差别。
for i = 1:299 % 生成带系统噪声原始数据
x(:,i+1) = A* x(:,i) + wk(:,i);
end
4万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
