问题描述

存在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(li(ki−1)+1)\ x(l_i(k_i-1)+1) x(li(ki−1)+1)移到左侧可以得到

那么根据观测方程我们可以得到：

在将上面推导的状态转移方程代入观测方程可以得到：
这里问什么要把x(li(ki−1)+1)\ x(l_i(k_i-1)+1) x(li(ki−1)+1)代入，个人认为：因为传感器i(i>1)\ i(i>1) i(i>1)不是在最细尺度上采样，那么对于没有采样数据的时刻点如果想要充分的利用观测数据可以用x(li(ki−1)+1)\ x(l_i(k_i-1)+1) x(li(ki−1)+1)与x(li(ki−1)+j)\ x(l_i(k_i-1)+j) x(li(ki−1)+j)的对应关系进行代换，将x(li(ki−1)+j)\ x(l_i(k_i-1)+j) x(li(ki−1)+j)还原到x(li(ki−1)+1)\ x(l_i(k_i-1)+1) x(li(ki−1)+1)上利用z(li(ki−1)+1)\ z(l_i(k_i-1)+1) z(li(ki−1)+1)进行修正（通过Ci\ C_i Ci可以看出）。

这里可以看出重构的观测方程参数，等式右侧第一项为γ(i,ki)Cix(li(ki−1)+j)\gamma(i,k_i)C_ix(l_i(k_i-1)+j)γ(i,ki)Cix(li(ki−1)+j),第二项与第三项的和为vi(k)\ v_i(k) vi(k)
根据这两项可以推出问题描述中给出的重构的参数Ci,Ri\ C_i,R_i Ci,Ri

如果不对状态转移矩阵进行简化那么重构的参数是怎么样的？

首先Γ,A(k),Gi(k)\Gamma,A(k),G_i(k)Γ,A(k),Gi(k)与上篇中重构得到的是一样的。
其次，Ci\ C_i Ci也不会发生变化(因为在算式的推导中Ci\ C_i Ci都是从等式右侧第一项中取)，变化的是vi\ v_i vi。（不确定理解的对不对）
下面进行推导：
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)\ 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,...,li\ j=3,4,...,l_i j=3,4,...,li有：
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)+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(li(ki−1)+1)x(l_i(k_i-1)+1)x(li(ki−1)+1)变换到左侧可以得到：
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)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)
将上式子代入观测方程，可以得出其中的vi(k)v_i(k)vi(k)这里就不给出代入后的式子了，直接写出得到的vi(k)v_i(k)vi(k)。

vi(k)={v(i,ki)k=li(ki−1)+1v(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)+j,j=2,3,...,liv_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
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)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,ki),v(i,ki)w(i,k_i),v(i,k_i)w(i,ki),v(i,ki)均为均值为0的高斯白噪声且具有统计无关性，这是上述推导中需要注意的。

融合算法

这里的M(k)\ M(k) M(k)集合实际上是丢失数据时刻的集合，那么如何理解上述集合的数学描述，个人理解：数据丢失即无法对预测得到的数据起到滤波修正的作用，从集合中可以看到E{[zi(k)−Ci(k)x^(k∣k−1)][zi(k)−Ci(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的一个概率分布的方差。那么如果这个方差无限接近于传感器的观测噪声矩阵Ri\ R_i Ri，那么此时就说明那么就说明观测值与预测值之间偏差的修正作用被传感器的观测噪声所掩盖，失去了滤波修正的作用，体现了数据丢失。

仿真实例

设定u(k)=0,Γ(k)=Iu(k)=0,\Gamma(k)=Iu(k)=0,Γ(k)=ISINS,GPS,SM分别为传感器1，2，3。
观测矩阵：

这里程序对第二种情况进行了仿真。针对本次仿真的设定可以采用简化模型推导得到的Ri(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 00 1 1 0 0 0 0 0 00 0 1 0 0 0 0 0 00 0 0 1 1 0.5 0 0 00 0 0 0 1 1 0 0 00 0 0 0 0 1 0 0 00 0 0 0 0 0 1 1 0.50 0 0 0 0 0 0 1 10 0 0 0 0 0 0 0 1];
%观测矩阵
C1=eye(9);
C2=[1 0 0 0 0 0 0 0 00 1 0 0 0 0 0 0 00 0 0 1 0 0 0 0 00 0 0 0 1 0 0 0 00 0 0 0 0 0 1 0 00 0 0 0 0 0 0 1 0];
C3=[1 0 0 0 0 0 0 0 00 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:Nif rem(i-1,n2)==0if 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^-4K1=zeros(9,9);elseK1=P1*C1'/(C1*P1*C1'+R1);end if shadow2<0.0001   %丢失几率为10^-4K2=zeros(9,6);elseK2=P1*C2'/(C2*P1*C2'+R2);end if shadow3<0.01   %丢失几率为10^-2K3=zeros(9,2);elseK3=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^-4K1=zeros(9,9);elseK1=P1*C1'/(C1*P1*C1'+R1);end if shadow2<0.0001   %丢失几率为10^-4K2=zeros(9,6);elseK2=P1*C2'/(C2*P1*C2'+R2);end %得到噪声矩阵%R3R_3=zeros(9,9);for n=1:j3R_3=(inv(A)^n)*Q*(inv(A')^n)+R_3;endR_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)};endelseif 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^-4K1=zeros(9,9);elseK1=P1*C1'/(C1*P1*C1'+R1);end if shadow3<0.01   %丢失几率为10^-2K3=zeros(9,2);elseK3=P1*C3'/(C3*P1*C3'+R3);end %得到噪声矩阵%R2R_2=zeros(9,9);for n=1:j2R_2=(inv(A)^n)*Q*(inv(A')^n)+R_2;endR_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^-4K1=zeros(9,9);elseK1=P1*C1'/(C1*P1*C1'+R1);end %得到噪声矩阵%R2R_2=zeros(9,9);for n=1:j2R_2=(inv(A)^n)*Q*(inv(A')^n)+R_2;endR_2=0.9999*C2*(inv(A)^(j2))*R_2*(C2*(inv(A)^(j2)))'+R2;%R3R_3=zeros(9,9);for n=1:j3R_3=(inv(A)^n)*Q*(inv(A')^n)+R_3;endR_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

数据随机丢失情况下多传感器多速率鲁棒融合估计相关推荐

在数据仓储的情况下进一步封装数据库基础操作，此版本为异步版本
1 /// <summary> 2 /// 在数据仓储的情况下进一步封装数据库基础操作,此版本为异步版本 Created by ZhangQC 2016.08.17 3 /// </ ...
百面机器学习 -- No.2 特征工程 -- 训练数据不足的情况下会带来什么问题，如何缓解？
训练数据不足的情况下会带来什么问题,如何缓解? 数据不足会带来什么问题如何解决 ? 数据不足会带来什么问题机器学习任务的问题,可以简单的理解成寻找最佳的拟合函数和最佳的泛化函数,拟合函数是用来学习 ...
对于大数据大流量情况下微软架构的水平扩展的遐想（瞎想）
最近回顾SAAS的书籍,书中的扩展架构都有点让我痴迷,但书中介绍的都是以Java,Apache,JBoss,Hadloop等技术实现负载均衡,大数据处理,对于微软架构并未提及,所以让我陷入无限遐想,夜 ...
java查询数据库大批量数据_数据库有百万数据量的情况下，分页查询的方法及其优化方式...
当需要从数据库查询的表有上万条记录的时候,一次性查询所有结果会变得很慢,特别是随着数据量的增加特别明显,这时需要使用分页查询.对于数据库分页查询,也有很多种方法和优化的点. 下面简单说一下我知道的一些 ...
银河麒麟桌面操作系统V10SP1如何在保留“数据盘”的情况下进行系统重装
文章目录背景初始环境介绍重装操作系统重装后的配置背景银河麒麟桌面操作系统V10SP1-2203是截至2023年4月份麒麟软件公司发布的桌面操作系统最新版本,安装此版本操作系统时有" ...
oracle备份信息在控制文件丢失,恢复之利用备份在所有控制文件丢失情况下恢复（一）...
如果全部控制文件丢失,但是包含以前控制文件的备份,这时可以利用备份的控制文件进行恢复,不过在恢复后需要以RESETLOGS方式打开数据库. 根据联机重做日志文件是否可用和数据文件是否是最新的可以分为四 ...
技巧 | 数据有缺失值情况下的一个处理方法
本篇分享一个处理技巧,也来自一位读者的咨询.示例数据如下所示(可在后台回复关键词示例数据获得). A2000-A2020五列为日期格式,其中包含缺失值:Year为某事件发生的年份.要求生成两列数据: ...
android定位数据在移动,android - 在我的Wi-Fi和移动数据关闭的情况下，是否可以仅通过GPS从经纬度获取地址？ - 堆栈内存溢出...
我正在测试一款在三星平板电脑中借助GPS进行纬度和经度的应用程序. 我能够从GPS单元捕获当前的纬度,经度. 现在,我想使用纬度经度从这些位置获取地址,而无需使用Internet即wi-fi,移动数据 ...
AI 大数据在数据隐私保护下如何普惠共享？CCF TF「联邦学习」研讨会给出了答案
雷锋网 AI 科技评论按:3 月 24 日,由 CCF 主办.微众银行和深圳大学微众金融科技研究院协办的第 14 期中国计算机学会技术前线研讨会于深圳大学科技楼二号报告厅圆满召开,研讨会的主题为「联邦 ...

数据随机丢失情况下多传感器多速率鲁棒融合估计