%************************************************************************%
%************************17階系統(tǒng)與12階系統(tǒng)二者的比較************************%
%************************************************************************%

clc
clear all;

%重力加速度

g=9.80; 

%各軸向的地速分量

Vx=8.0;
Vy=8.0; 
Vz=0.0;

%地球的半徑

R=6371020.0; 

%當?shù)氐木暥冉?
angle=pi/4; 

%地球自轉角速度

Wie=7.2921158e-5; 

%所測到各軸向的比力信息

fx=(2*Wie*cos(angle)+Vx/R)*Vz-(2*Wie*sin(angle)+Vx*tan(angle)/R)*Vy;
fy=(2*Wie*sin(angle)+Vx*tan(angle)/R)*Vx+Vy*Vz/R;    
fz=-(2*Wie*cos(angle)+Vx/R)*Vx-Vy^2/R+g;

%系統(tǒng)的狀態(tài)矩陣A1陣

A1=zeros(17);

A1(1,2)= Vx*tan(angle)/R+Wie*sin(angle);
A1(1,3)=-Vx/R-Wie*cos(angle);
A1(1,8)=-1/R;
A1(1,10)=-1;
A1(2,1)=-Vx*tan(angle)/R-Wie*sin(angle);
A1(2,3)=-Vy/R;
A1(2,7)=1/R;
A1(2,9)=-Wie*sin(angle);
A1(2,11)=-1;
A1(3,1)=Vx/R+Wie*cos(angle);
A1(3,2)=Vy/R;
A1(3,7)=tan(angle)/R;
A1(3,9)=Wie*cos(angle)+Vx*sec(angle)^2/R;
A1(3,12)=-1;
A1(4,10)=1;
A1(4,13)=1;
A1(5,11)=1;
A1(5,14)=1;
A1(6,12)=1;
A1(6,15)=1;
A1(7,2)=-fz;
A1(7,3)=fy; 
A1(7,7)=(Vy*tan(angle)-Vz)/R;
A1(7,8)=2*Wie*sin(angle)+Vx*tan(angle)/R; 
A1(7,9)=2*Wie*sin(angle)*Vz+(2*Wie*cos(angle)+Vx*sec(angle)^2/R)*Vy;
A1(7,16)=1;
A1(8,1)=fz;
A1(8,3)=-fx;
A1(8,7)=-2*Wie*sin(angle)-2*Vx*tan(angle)/R;
A1(8,8)=-Vz/R;
A1(8,9)=-2*Wie*cos(angle)*Vx-Vx^2*sec(angle)^2/R;
A1(8,17)=1;
A1(9,8)=1/R;

%系統(tǒng)的觀測矩陣C1陣

C1=zeros(5,17);

C1(1,7)=1;
C1(2,8)=1;
C1(3,1)=-1;
C1(3,4)=1;
C1(4,2)=-1;
C1(4,5)=1;
C1(5,3)=-1;
C1(5,6)=1;

%系統(tǒng)方程離散化

B1=zeros(17,1);
D1=zeros(5,1);

[a1,b1,c1,d1]=c2dm(A1,B1,C1,D1,2);

%kalman濾波方程的定義

estX1=zeros(17,127);
realX1=zeros(17,127);
PP1=zeros(17,17,126);
P1=zeros(17,17,127);
K1=zeros(17,5,127);
Z1=zeros(5,127);
estXX1=zeros(17,501);
CP1=zeros(17,17,501);

%系統(tǒng)的觀測噪聲的協(xié)方差強度陣

RR1=zeros(5);

RR1(1,1)=0.01;
RR1(2,2)=0.01;
RR1(3,3)=(pi/(60*180))^2;
RR1(4,4)=(pi/(60*180))^2;
RR1(5,5)=(3*pi/(60*180))^2;

%系統(tǒng)噪聲方差陣Q1陣

Q1=zeros(17);

Q1(1,1)=(0.01*pi/(180*3600))^2;
Q1(2,2)=(0.01*pi/(180*3600))^2;
Q1(3,3)=(0.01*pi/(180*3600))^2;
Q1(4,4)=(0.01*pi/(180*3600))^2;   
Q1(5,5)=(0.01*pi/(180*3600))^2;
Q1(6,6)=(0.01*pi/(180*3600))^2;
Q1(7,7)=(1e-5*g)^2;              
Q1(8,8)=(1e-5*g)^2; 

%系統(tǒng)噪聲

w1=[0.01*pi/(180*3600);0.01*pi/(180*3600);0.01*pi/(180*3600);0.01*pi/(180*3600);0.01*pi/(180*3600);0.01*pi/(180*3600);(1e-5*g);(1e-5*g);0;0;0;0;0;0;0;0;0];

%量測噪聲

v1=[0.1 0.1 pi/(60*180) pi/(60*180) 3*pi/(60*180)]';

%狀態(tài)變量的初始值

estX1(:,1)=[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]';

realX1(:,1)=[3*pi/(60*180) 3*pi/(60*180) 5*pi/(60*180) 5*pi/(60*180) 5*pi/(60*180) 6*pi/(60*180) 0.1 0.1 2*pi/(60*180) 0.1*pi/(180*3600) 0.1*pi/(180*3600) 0.1*pi/(180*3600) 0 0 0 1e-4*g 1e-4*g]';

%協(xié)方差矩陣的初始值

P1(1,1,1)=(3*pi/(60*180))^2;
P1(2,2,1)=(3*pi/(60*180))^2;
P1(3,3,1)=(5*pi/(60*180))^2;
P1(4,4,1)=(5*pi/(60*180))^2;
P1(5,5,1)=(5*pi/(60*180))^2;
P1(6,6,1)=(6*pi/(60*180))^2;
P1(7,7,1)=0.01;
P1(8,8,1)=0.01;
P1(9,9,1)=(2*pi/(60*180))^2;
P1(10,10,1)=(0.1*pi/(180*3600))^2;
P1(11,11,1)=(0.1*pi/(180*3600))^2;
P1(12,12,1)=(0.1*pi/(180*3600))^2;
P1(13,13,1)=0.01;
P1(14,14,1)=0.125^2;
P1(15,15,1)=0.001^2;
P1(16,16,1)=((1e-4)*g)^2;
P1(17,17,1)=((1e-4)*g)^2;

CP1(:,:,1)=P1(:,:,1);

for j=1:500
    
    for i=2:127
        
        PP1(:,:,i)=a1*P1(:,:,i-1)*a1'+Q1;
        K1(:,:,i)=PP1(:,:,i)*c1'*inv(c1*PP1(:,:,i)*c1'+RR1);
        ww=w1*rand(1);
        realX1(:,i)=a1*realX1(:,i-1)+ww;
        vv=v1*rand(1);
        Z1(:,i)=c1*realX1(:,i)+vv;
        estX1(:,i)=a1*estX1(:,i-1)+K1(:,:,i)*(Z1(:,i)-c1*a1*estX1(:,i-1));
        P1(:,:,i)=PP1(:,:,i)-K1(:,:,i)*c1*PP1(:,:,i);
        
    end
    
    P1(:,:,1)=P1(:,:,127);
    realX1(:,1)=realX1(:,127);
    estX1(:,1)=estX1(:,127);
    estXX1(:,j+1)=estX1(:,127);
    CP1(:,:,j+1)=P1(:,:,127);
    
end

for i=1:501
    
    est11(i)=estXX1(1,i)*180*60/pi;
    est12(i)=estXX1(2,i)*180*60/pi;
    est13(i)=estXX1(3,i)*180*60/pi;
    est14(i)=estXX1(4,i)*180*60/pi;
    est15(i)=estXX1(5,i)*180*60/pi;
    est16(i)=estXX1(6,i)*180*60/pi;
    est17(i)=estXX1(7,i);
    est18(i)=estXX1(8,i);
    est19(i)=estXX1(9,i)*180*60/pi;
    est110(i)=estXX1(10,i)*180*3600/pi;
    est111(i)=estXX1(11,i)*180*3600/pi;
    est112(i)=estXX1(12,i)*180*3600/pi;
    est113(i)=estXX1(13,i);
    est114(i)=estXX1(14,i);
    est115(i)=estXX1(15,i);
    est116(i)=estXX1(16,i)/(1e-6*g);
    est117(i)=estXX1(17,i)/(1e-6*g);
    
    PPP11(i)=CP1(1,1,i)*(180*60/pi)^2;
    PPP12(i)=CP1(2,2,i)*(180*60/pi)^2;
    PPP13(i)=CP1(3,3,i)*(180*60/pi)^2;
    PPP14(i)=CP1(4,4,i)*(180*60/pi)^2;
    PPP15(i)=CP1(5,5,i)*(180*60/pi)^2;
    PPP16(i)=CP1(6,6,i)*(180*60/pi)^2;
    PPP17(i)=CP1(7,7,i);
    PPP18(i)=CP1(8,8,i);
    PPP19(i)=CP1(9,9,i)*(180*60/pi)^2;
    PPP110(i)=CP1(10,10,i)*(180*3600/pi)^2;
    PPP111(i)=CP1(11,11,i)*(180*3600/pi)^2;
    PPP112(i)=CP1(12,12,i)*(180*3600/pi)^2;
    PPP113(i)=CP1(13,13,i);
    PPP114(i)=CP1(14,14,i);
    PPP115(i)=CP1(15,15,i);
    PPP116(i)=CP1(16,16,i)/(1e-6*g)^2;
    PPP117(i)=CP1(17,17,i)/(1e-6*g)^2;
    
end

t=0:252/3600:35;

figure(1)

subplot(3,3,1)
plot(t,est11,'b');
xlabel('time/h');
grid on;

subplot(3,3,2)
plot(t,est12,'b');
xlabel('time/h');
grid on;

subplot(3,3,3)
plot(t,est13,'b');
xlabel('time/h');
grid on;

subplot(3,3,4)
plot(t,est14,'b');
xlabel('time/h');
grid on;

subplot(3,3,5)
plot(t,est15,'b');
xlabel('time/h');
grid on;

subplot(3,3,6)
plot(t,est16,'b');
xlabel('time/h');
grid on;

subplot(3,3,7)
plot(t,est17,'b');
xlabel('time/h');
grid on;

subplot(3,3,8)
plot(t,est18,'b');
xlabel('time/h');
grid on;

subplot(3,3,9)
plot(t,est19,'b');
xlabel('time/h');
grid on;

figure(2)

subplot(3,3,1)
plot(t,est113,'b');
xlabel('time/h');
grid on;

subplot(3,3,2)
plot(t,est114,'b');
xlabel('time/h');
grid on;

subplot(3,3,3)
plot(t,est115,'b');
xlabel('time/h');
grid on;

%加速度計的漂移誤差

ax2=1e-4*g; 
ay2=1e-4*g; 

%陀螺儀漂移誤差

ex2=0.1*pi/(180*3600);
ey2=0.1*pi/(180*3600);
ez2=0.1*pi/(180*3600);

%求出A2陣

A2=zeros(12);

A2(1,2)= Vx*tan(angle)/R+Wie*sin(angle);
A2(1,3)=-Vx/R-Wie*cos(angle);
A2(1,8)=-1/R;
A2(2,1)=-Vx*tan(angle)/R-Wie*sin(angle);
A2(2,3)=-Vy/R;
A2(2,7)=1/R;
A2(2,9)=-Wie*sin(angle);
A2(3,1)=Vx/R+Wie*cos(angle);
A2(3,2)=Vy/R;
A2(3,7)=tan(angle)/R;
A2(3,9)=Wie*cos(angle)+Vx*sec(angle)^2/R;
A2(4,10)=1;
A2(5,11)=1;
A2(6,12)=1;
A2(7,2)=-fz;
A2(7,3)=fy; 
A2(7,7)=(Vy*tan(angle)-Vz)/R;
A2(7,8)=2*Wie*sin(angle)+Vx*tan(angle)/R; 
A2(7,9)=2*Wie*sin(angle)*Vz+(2*Wie*cos(angle)+Vx*sec(angle)^2/R)*Vy;
A2(8,1)=fz;
A2(8,3)=-fx;
A2(8,7)=-2*Wie*sin(angle)-2*Vx*tan(angle)/R;
A2(8,8)=-Vz/R;
A2(8,9)=-2*Wie*cos(angle)*Vx-Vx^2*sec(angle)^2/R;
A2(9,8)=1/R;

B2=eye(12);

D2=zeros(5,12);

%求出C2陣

C2=zeros(5,12);

C2(1,7)=1;
C2(2,8)=1;
C2(3,1)=-1;
C2(3,4)=1;
C2(4,2)=-1;
C2(4,5)=1;
C2(5,3)=-1;
C2(5,6)=1;

%離散化

[a2,b2,c2,d2]=c2dm(A2,B2,C2,D2,2);

%kalman濾波方程的定義

estX2=zeros(12,127);
realX2=zeros(12,127);
PP2=zeros(12,12,126);
P2=zeros(12,12,127);
K2=zeros(12,5,127);
Z2=zeros(5,127);
estXX2=zeros(12,501);
CP2=zeros(12,12,501);

%系統(tǒng)的觀測噪聲的協(xié)方差強度陣

RR2=zeros(5);

RR2(1,1)=0.01;
RR2(2,2)=0.01;
RR2(3,3)=(pi/(60*180))^2;
RR2(4,4)=(pi/(60*180))^2;
RR2(5,5)=(3*pi/(60*180))^2;

%置系統(tǒng)噪聲方差陣Q2陣

Q2=zeros(12);

Q2(1,1)=(0.01*pi/(180*3600))^2;
Q2(2,2)=(0.01*pi/(180*3600))^2;
Q2(3,3)=(0.01*pi/(180*3600))^2;
Q2(4,4)=(0.01*pi/(180*3600))^2;    
Q2(5,5)=(0.01*pi/(180*3600))^2;
Q2(6,6)=(0.01*pi/(180*3600))^2;
Q2(7,7)=(1e-5*g)^2;             
Q2(8,8)=(1e-5*g)^2;

%系統(tǒng)噪聲

w2=[0.01*pi/(180*3600) 0.01*pi/(180*3600) 0.01*pi/(180*3600) 0.01*pi/(180*3600) 0.01*pi/(180*3600) 0.01*pi/(180*3600) 1e-5*g 1e-5*g 0 0 0 0]';

%量測噪聲

v2=[0.1 0.1 pi/(60*180) pi/(60*180) 3*pi/(60*180)]';

%狀態(tài)變量的初始值

estX2(:,1)=[0 0 0 0 0 0 0 0 0 0 0 0 ]';

realX2(:,1)=[3*pi/(60*180) 3*pi/(60*180) 5*pi/(60*180) 5*pi/(60*180) 5*pi/(60*180) 6*pi/(60*180) 0.1 0.1 2*pi/(60*180) 0 0 0]';

%協(xié)方差矩陣的初始值

P2(1,1,1)=(3*pi/(60*180))^2;
P2(2,2,1)=(3*pi/(60*180))^2;
P2(3,3,1)=(5*pi/(60*180))^2;
P2(4,4,1)=(5*pi/(60*180))^2;
P2(5,5,1)=(5*pi/(60*180))^2;
P2(6,6,1)=(6*pi/(60*180))^2;
P2(7,7,1)=0.01;
P2(8,8,1)=0.01;
P2(9,9,1)=(2*pi/(60*180))^2;
P2(10,10,1)=0.1^2;
P2(11,11,1)=0.125^2;
P2(12,12,1)=0.001^2;

%確定的干擾項

CP2(:,:,1)=P2(:,:,1);

f2=[-ex2 -ey2 -ez2 ex2 ey2 ez2 ax2 ay2 0 0 0 0]';

for j=1:500
    
   for i=2:127
       
     PP2(:,:,i)=a2*P2(:,:,i-1)*a2'+Q2;
     K2(:,:,i)=PP2(:,:,i)*c2'*inv(c2*PP2(:,:,i)*c2'+RR2);
     www=w2*rand(1);
     realX2(:,i)=a2*realX2(:,i-1)+www;
     vvv=v2*rand(1);
     Z2(:,i)=c2*realX2(:,i)+vvv;
     estX2(:,i)=a2*estX2(:,i-1)+b2*f2+K2(:,:,i)*(Z2(:,i)-c2*(a2*estX2(:,i-1)+b2*f2));
     P2(:,:,i)=PP2(:,:,i)-K2(:,:,i)*c2*PP2(:,:,i);
     
   end
   
     P2(:,:,1)=P2(:,:,127);
     realX2(:,1)=realX2(:,127);
     estX2(:,1)=estX2(:,127);
     estXX2(:,j+1)=estX2(:,127);
     CP2(:,:,j+1)=P2(:,:,127);
     
end

for i=1:501
    
    est21(i)=estXX2(1,i)*180*60/pi;
    est22(i)=estXX2(2,i)*180*60/pi;
    est23(i)=estXX2(3,i)*180*60/pi;
    est24(i)=estXX2(4,i)*180*60/pi;
    est25(i)=estXX2(5,i)*180*60/pi;
    est26(i)=estXX2(6,i)*180*60/pi;
    est27(i)=estXX2(7,i);
    est28(i)=estXX2(8,i);
    est29(i)=estXX2(9,i)*180*60/pi;
    est210(i)=estXX2(10,i);
    est211(i)=estXX2(11,i);
    est212(i)=estXX2(12,i);
    
    PPP21(i)=CP2(1,1,i)*(180*60/pi)^2;
    PPP22(i)=CP2(2,2,i)*(180*60/pi)^2;
    PPP23(i)=CP2(3,3,i)*(180*60/pi)^2;
    PPP24(i)=CP2(4,4,i)*(180*60/pi)^2;
    PPP25(i)=CP2(5,5,i)*(180*60/pi)^2;
    PPP26(i)=CP2(6,6,i)*(180*60/pi)^2;
    PPP27(i)=CP2(7,7,i);
    PPP28(i)=CP2(8,8,i);
    PPP29(i)=CP2(9,9,i)*(180*60/pi)^2;
    PPP210(i)=CP2(10,10,i);
    PPP211(i)=CP2(11,11,i);
    PPP212(i)=CP2(12,12,i);
           
end

t=0:252/3600:35;

figure(3)

subplot(3,3,1)
plot(t,est21,'b');
xlabel('time/h');
grid on;

subplot(3,3,2)
plot(t,est22,'b');
xlabel('time/h');
grid on;

subplot(3,3,3)
plot(t,est23,'b');
xlabel('time/h');
grid on;

subplot(3,3,4)
plot(t,est24,'b')
xlabel('time/h');
grid on;

subplot(3,3,5)
plot(t,est25,'b');
xlabel('time/h');
grid on;

subplot(3,3,6)
plot(t,est26,'b');
xlabel('time/h');
grid on;

subplot(3,3,7)
plot(t,est27,'b');
xlabel('time/h');
grid on;

subplot(3,3,8)
plot(t,est28,'b');
xlabel('time/h');
grid on;

subplot(3,3,9)
plot(t,est29,'b');
xlabel('time/h');
grid on;

figure(4)

subplot(3,3,1)
plot(t,est210,'b');
xlabel('time/h');
grid on;

subplot(3,3,2)
plot(t,est211,'b');
xlabel('time/h');
grid on;

subplot(3,3,3)
plot(t,est212,'b');
xlabel('time/h');
grid on;



%畫出狀態(tài)變量的估計值的曲線圖

t=0:252/3600:35;

figure(5)

subplot(3,3,1)
plot(t,est11-est21,'b');
xlabel('time/h');
grid on;

subplot(3,3,2)
plot(t,est12-est22,'b');
xlabel('time/h');
grid on;

subplot(3,3,3)
plot(t,est13-est23,'b');
xlabel('time/h');
grid on;

subplot(3,3,4)
plot(t,est14-est24,'b');
xlabel('time/h');
grid on;

subplot(3,3,5)
plot(t,est15-est25,'b');
xlabel('time/h');
grid on;

subplot(3,3,6)
plot(t,est16-est26,'b');
xlabel('time/h');
grid on;

subplot(3,3,7)
plot(t,est17-est27,'b');
xlabel('time/h');
grid on;

subplot(3,3,8)
plot(t,est18-est28,'b');
xlabel('time/h');
grid on;

subplot(3,3,9)
plot(t,est19-est29,'b');
xlabel('time/h');
grid on;

figure(6)

subplot(3,3,1)
plot(t,est113-est210,'b');
xlabel('time/h');
grid on;

subplot(3,3,2)
plot(t,est114-est211,'b');
xlabel('time/h');
grid on;

subplot(3,3,3)
plot(t,est115-est212,'b');
xlabel('time/h');
grid on;

%協(xié)方差矩陣的仿真

figure(7)
subplot(3,3,1)
plot(t,PPP11,'b',t,PPP21,'r');
xlabel('time/h');
grid on;


subplot(3,3,2)
plot(t,PPP12,'b',t,PPP22,'r');
xlabel('time/h');
grid on;


subplot(3,3,3)
plot(t,PPP13,'b',t,PPP23,'r');
xlabel('time/h');
grid on;


subplot(3,3,4)
plot(t,PPP14,'b',t,PPP24,'r');
xlabel('time/h');
grid on;


subplot(3,3,5)
plot(t,PPP15,'b',t,PPP25,'r');
xlabel('time/h');
grid on;


subplot(3,3,6)
plot(t,PPP16,'b',t,PPP26,'r');
xlabel('time/h');
grid on;


subplot(3,3,7)
plot(t,PPP17,'b',t,PPP27,'r');
xlabel('time/h');
grid on;


subplot(3,3,8)
plot(t,PPP18,'b',t,PPP28,'r');
xlabel('time/h');
grid on;


subplot(3,3,9)
plot(t,PPP19,'b',t,PPP29,'r');
xlabel('time/h');
grid on;

figure(8)

subplot(3,3,1)
plot(t,PPP110,'b',t,PPP210,'r');
xlabel('time/h');
grid on;


subplot(3,3,2)
plot(t,PPP112,'b',t,PPP211,'r');
xlabel('time/h');
grid on;


subplot(3,3,3)
plot(t,PPP112,'b',t,PPP212,'r');
xlabel('time/h');
grid on;