# Problem description

There are N sensors for observation, and there is a certain probability that the observation data will be lost

In short, where γ \ gamma γ is a random number (non-zero is 1), 0 represents data loss, and 1 represents data existence. Other multi-sensor and multi rate representations are consistent with the previous article.
The following also rewrites the multi rate model into a single model.

among

# Algorithm deduction

The derivation of the reconstruction parameters of the state transition equation of multi-rate and multi-sensor has been described in detail in the previous paper. Here, only the observation equation, i.e. different parts, are discussed.
In order to simplify the derivation of the state transition equation, the control term is set to 0, and the noise gain matrix γ \ Gamma γ is set to the unit matrix, that is, x(k+1)=A(k)x(k)+w(k) + X (k) + x(k+1)=A(k)x(k)+w(k) x (K + 1) = a (k) x (k) + W (k)

In the same way, we can write:
At this time, move x (l i (ki − 1) + 1) \ x (L ﹣ I (K ﹣ i-1) + 1) x(li(ki − 1) + 1) to the left

Then according to the observation equation, we can get:

By substituting the state transfer equation derived above into the observation equation, we can get:
- x (l i (ki − 1)+j)\ I (k i-1) + 1) z(li(ki − 1) + 1) is corrected (as can be seen from Ci \ C CI).

Here, we can see the parameters of the reconstructed observation equation. On the right side of the equation, the first term is γ (i,ki)Cix(li(ki − 1)+j), gamma (I, K ｝ I) C ｝ I x (L ｝ i-1)+j) γ (i,ki) Ci x(li (ki − 1) + J, and the sum of the second term and the third term is vi (k) \ V ｝ I (k) vi (k)
According to these two parameters, we can deduce the reconstruction parameters Ci, Ri \ C, R \ i ci, Ri given in the problem description

If the state transition matrix is not simplified, what are the parameters of reconstruction?

# Fusion algorithm

The M(k) m (k) M(k) m (k) set here is actually a set of lost data times. How to understand the mathematical description of the above set? Personal understanding: data loss means that it can not filter and correct the predicted data. From the set, we can see that E{[zi(k) - Ci(k)x^(k ∣ K − 1)][zi(k) - Ci(k)x^(k ∣ K − 1)]T}\ Noise matrix, because in the sense of probability, the mean value of the observed value and the predicted value is regarded as the true value, but the actual data are all samples In terms of probability distribution, the above formula can be regarded as the variance of a probability distribution whose mean value is 0. If the variance is infinitely close to the observation noise matrix Ri \ R ﹤ Ri of the sensor, then it means that the correction effect of the deviation between the observation value and the predicted value is covered by the observation noise of the sensor, and the filtering correction effect is lost, reflecting the data loss.

# Simulation example

Set u(k)=0, γ (k) = IU (k) = 0, gamma (k) = IU (k) = 0, γ (k) = isins, GPS and SM as sensors 1, 2 and 3 respectively.
Observation matrix:

The second case is simulated by the program here. For the setting of this simulation, the RI (k) r UUI (k) RI (k) derived from the simplified model can be used

```%%%===========Robust fusion design of multi rate sensors with random packet loss========%%%
clc;
clear all;
close all;
%% Data generation
%Let an aircraft fly at a fixed altitude at approximately uniform acceleration(Assume the direction is northeast)
%State matrix x by9x1   The states are respectively in the east direction/Eastward velocity/East acceleration/Northward position/Northward velocity/North acceleration/Direction of heaven/Radial velocity/Celestial acceleration
%x=[xe ve ae xn vn an xu vu au]';
%Generate the actual flight data. The flight acceleration is5m/s^2
N=500;%time span
n1=1;      %SINS Sampling interval
n2=2;      %GPS Sampling interval
n3=10;     %SM Sampling interval
M=lcm(n2,n3);%The minimum common multiple of sampling interval is the fusion period
M1=M/n1;
M2=M/n2;
M3=M/n3;
t=1:1:N ;  %time series
X0=[0 0 0 0 300 0 800 0 0 ];%Initial state
P0=diag([100,16,1,100,16,1,100,16,1]);
%Actual data
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);
%Generation of observation data
%1.Simultaneous interpreting noise due to different sensor pairs/The measurement noise of velocity and acceleration is not the same, so noise is added one by one.
%SINS Northeast celestial position/speed/Acceleration inertial navigation
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 Northeast celestial position/speed
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 Scene matching northeast position data
SM_xe=xe+randn(1,N)*sqrt(100);
SM_xn=xn+randn(1,N)*sqrt(100);

%observed data
%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);
%% transition matrix/Observation matrix/Noise matrix/     Sparse matrix can be used here to save memory A=sparse(r,c,S)
%transition matrix
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];
%Observation matrix
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];
%Noise matrix
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;

%% Wave filtering
%Suppose that the time when three sensors get data synchronously is 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
%In this case, all three sensors get data
%Random loss number generation
%Forecast
temp=A*cell2mat(X(i));
P1=A*P0*A'+Q;
%Data loss judgment
if shadow1<0.0001   %The probability of loss is10^-4
K1=zeros(9,9);
else
K1=P1*C1'/(C1*P1*C1'+R1);
end
if shadow2<0.0001   %The probability of loss is10^-4
K2=zeros(9,6);
else
K2=P1*C2'/(C2*P1*C2'+R2);

end
if shadow3<0.01   %The probability of loss is10^-2
K3=zeros(9,2);
else
K3=P1*C3'/(C3*P1*C3'+R3);

end
%K It's worth it
%Estimate
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);
%Fusion
P0=inv(p_1+p_2+p_3);
X(i+1)={P0*(p_1*x1+p_2*x2+p_3*x3)};

else
%In this case, only SINS And GPS Get data
%Random loss number generation
%Forecast
temp=A*cell2mat(X(i));
P1=A*P0*A'+Q;
j3=rem(i-1,n3);
%Data loss judgment
if shadow1<0.0001   %The probability of loss is10^-4
K1=zeros(9,9);
else
K1=P1*C1'/(C1*P1*C1'+R1);
end
if shadow2<0.0001   %The probability of loss is10^-4
K2=zeros(9,6);
else
K2=P1*C2'/(C2*P1*C2'+R2);

end
%Get noise matrix
%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;
%Gain matrix obtained
K3=P1*(C3*(inv(A)^(j3)))'/(C3*inv(A)^(j3)*P1*(C3*(inv(A)^(j3)))'+R_3);

%estimate
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);
%Fusion
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
%In this case SINS And SM Sensor information
%Random loss number generation
%Forecast
temp=A*cell2mat(X(i));
P1=A*P0*A'+Q;
j2=rem(i-1,n2);
%Data loss judgment
if shadow1<0.0001   %The probability of loss is10^-4
K1=zeros(9,9);
else
K1=P1*C1'/(C1*P1*C1'+R1);
end
if shadow3<0.01   %The probability of loss is10^-2
K3=zeros(9,2);
else
K3=P1*C3'/(C3*P1*C3'+R3);

end
%Get noise matrix
%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;
%Gain matrix obtained
K2=P1*(C2*(inv(A)^(j2)))'/(C2*inv(A)^(j2)*P1*(C2*(inv(A)^(j2)))'+R_2);

%estimate
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);
%Fusion

P0=inv(p_1+p_2+p_3);
X(i+1)={P0*(p_1*x1+p_2*x2+p_3*x3)};

else
%In this case, only SINS data
%Random loss number generation
%Forecast
temp=A*cell2mat(X(i));
P1=A*P0*A'+Q;
j2=rem(i-1,n2);
j3=rem(i-1,n3);
%Data loss judgment
if shadow1<0.0001   %The probability of loss is10^-4
K1=zeros(9,9);
else
K1=P1*C1'/(C1*P1*C1'+R1);
end
%Get noise matrix
%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;
%Gain matrix obtained
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);

%estimate
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);
%Fusion
P0=inv(p_1+p_2+p_3);
X(i+1)={P0*(p_1*x1+p_2*x2+p_3*x3)};
end
end
end

%% Image rendering
l=1:1:500;
X=cell2mat(X);
X=X(1:9,2:501);
%Actual vs. estimated
subplot(3,1,1)
plot(l,X(1,:),'b',l,xe,'r');
title('Eastward position');
legend('Estimated value','actual value');
xlabel('t');
ylabel('s');
subplot(3,1,2)
plot(l,X(2,:),'b',l,ve,'r');
title('Eastward velocity');
legend('Estimated value','actual value');
xlabel('t');
ylabel('v');
subplot(3,1,3)
plot(l,X(3,:),'b',l,ae,'r');
title('East acceleration');
legend('Estimated value','actual value');
xlabel('t');
ylabel('a');
figure(2)
subplot(3,1,1)
plot(l,X(4,:),'b',l,xn,'r');
title('Northward position');
legend('Estimated value','actual value');
xlabel('t');
ylabel('s');
subplot(3,1,2)
plot(l,X(5,:),'b',l,vn,'r');
title('Northward velocity');
legend('Estimated value','actual value');
xlabel('t');
ylabel('v');
subplot(3,1,3)
plot(l,X(6,:),'b',l,an,'r');
title('North acceleration');
legend('Estimated value','actual value');
xlabel('t');
ylabel('a');
figure(3)
subplot(3,1,1)
plot(l,X(7,:),'b',l,xu,'r');
title('Direction of heaven');
legend('Estimated value','actual value');
xlabel('t');
ylabel('s');
subplot(3,1,2)
plot(l,X(8,:),'b',l,vu,'r');
legend('Estimated value','actual value');
xlabel('t');
ylabel('v');
subplot(3,1,3)
plot(l,X(9,:),'b',l,au,'r');
title('Celestial acceleration');
legend('Estimated value','actual value');
xlabel('t');
ylabel('a');

%error
figure(4)
subplot(3,1,1)
plot(l,X(1,:)-xe,'r');
title('Absolute error of East position');
xlabel('t');
ylabel('δ');
subplot(3,1,2)
plot(l,X(2,:)-ve,'r');
title('Absolute error of East velocity');
xlabel('t');
ylabel('δ');
subplot(3,1,3)
plot(l,X(3,:)-ae,'r');
title('Absolute error of acceleration in east direction');
xlabel('t');
ylabel('δ');

figure(5)
subplot(3,1,1)
plot(l,X(4,:)-xn,'r');
title('Absolute error of North position');
xlabel('t');
ylabel('δ');
subplot(3,1,2)
plot(l,X(5,:)-vn,'r');
title('Absolute error of northward velocity');
xlabel('t');
ylabel('δ');
subplot(3,1,3)
plot(l,X(6,:)-an,'r');
title('North acceleration absolute error');
xlabel('t');
ylabel('δ');

figure(6)
subplot(3,1,1)
plot(l,X(7,:)-xu,'r');
title('Absolute error of celestial position');
xlabel('t');
ylabel('δ');
subplot(3,1,2)
plot(l,X(8,:)-vu,'r');
title('Absolute error of celestial velocity');
xlabel('t');
ylabel('δ');
subplot(3,1,3)
plot(l,X(9,:)-au,'r');
title('Absolute error of celestial acceleration');
xlabel('t');
ylabel('δ');

```
Published 15 original articles, won praise 4, visited 1312

Posted by nwoottonn on Wed, 26 Feb 2020 22:49:51 -0800