%***********************************************************************%
%***********************15階系統(tǒng)的秩和kalman濾波************************%
%***********************************************************************%

clc
clear all;

%重力加速度

g=9.80; 

%各軸向的地速分量

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

%地球的半徑

R=6371020; 

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

%地球自轉(zhuǎn)角速度

Wie=7.2921158e-5; 

%加速度計各軸向的零位誤差

ax=1e-4*g; 
ay=1e-4*g; 

%陀螺儀各軸向的漂移誤差

ex=pi/(180*3600); 
ey=pi/(180*3600); 
ez=pi/(180*3600);

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

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)矩陣A陣

A=zeros(15);

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

%求出離散化前的系統(tǒng)的觀測矩陣C陣

C=zeros(5,15);

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

%求系統(tǒng)的秩

E1=[C;C*A;C*A^2;C*A^3;C*A^4;C*A^5;C*A^6;C*A^7;C*A^8;C*A^9;C*A^10;C*A^11;C*A^12;C*A^13;C*A^14];
E2=[C;C*A;C*A^2];
E3=[C;C*A];
E4=[C*A^2];
F1=[0 0 1 0 0 0 0 0 0 0 0 0 0 0 0];
F2=[0 0 0 0 0 0 0 0 1 0 0 0 0 0 0];
E5=[C;C*A;E4(1,:);E4(2,:);E4(3,:);F1;F2];
Y1=rank(E1);
Y2=rank(E2);
Y3=rank(E3);
Y4=rank(E4);
Y5=rank(E5);
fprintf('%d,%d,%d,%d,%d\n',Y1,Y2,Y3,Y4,Y5);

%離散化前的系統(tǒng)的奇異值分解

[U1,S1,V1]=svd(E1);
[U2,S2,V2]=svd(E2);


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

B=zeros(15,1);
D=zeros(5,1);
[a,b,c,d]=c2dm(A,B,C,D,2,'foh');

%求出離散化后系統(tǒng)的秩

e1=[c;c*a;c*a^2;c*a^3;c*a^4;c*a^5;c*a^6;c*a^7;c*a^8;c*a^9;c*a^10;c*a^11;c*a^12;c*a^13;c*a^14];
e2=[c;c*a;c*a^2];
e3=[c;c*a];
e4=[c*a^2];
f1=[0 0 1 0 0 0 0 0 0 0 0 0 0 0 0];
f2=[0 0 0 0 0 0 0 0 1 0 0 0 0 0 0];
e5=[c;c*a;e4(1,:);e4(2,:);e4(3,:);f1;f2];
y1=rank(e1);
y2=rank(e2);
y3=rank(e3);
y4=rank(e4);
y5=rank(e5);
fprintf('%d,%d,%d,%d,%d',y1,y2,y3,y4,y5);

%離散化后的系統(tǒng)的奇異值分解

[u1,s1,v1]=svd(e1);
[u2,s2,v2]=svd(e2);

%kalman濾波方程的定義

estX=zeros(15,127);
realX=zeros(15,127);
PP=zeros(15,15,126);
P=zeros(15,15,127);
K=zeros(15,5,127);
Z=zeros(5,127);
estXX=zeros(15,501);
CP=zeros(15,15,501);

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

R=zeros(5);
R(1,1)=0.01;
R(2,2)=0.01;
R(3,3)=0.01;
R(4,4)=0.01;
R(5,5)=0.01;

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

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

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

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

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

P(1,1,1)=(pi/180)^2;
P(2,2,1)=(pi/180)^2;
P(3,3,1)=(pi/180)^2;
P(4,4,1)=(pi/180)^2;
P(5,5,1)=(pi/180)^2;
P(6,6,1)=(pi/180)^2;
P(7,7,1)=0.01;
P(8,8,1)=0.01;
P(9,9,1)=0;
P(10,10,1)=(0.02*pi/180)^2;
P(11,11,1)=(0.02*pi/180)^2;
P(12,12,1)=(0.02*pi/180)^2;
P(13,13,1)=0.1^2;
P(14,14,1)=0.125^2;
P(15,15,1)=0.0;

%求出經(jīng)過35個小時后系統(tǒng)狀態(tài)變量估計值 協(xié)方差矩陣 增益矩陣及觀測值 

for j=1:500
    
    for i=2:127
        
        PP(:,:,i)=a*P(:,:,i-1)*a';
        K(:,:,i)=PP(:,:,i)*c'*inv(c*PP(:,:,i)*c'+R);
        Z(:,i)=c*realX(:,i)+wgn(5,1,0.1);
        estX(:,i)=a*estX(:,i-1)+K(:,:,i)*(Z(:,i)-c*a*estX(:,i-1));
        P(:,:,i)=PP(:,:,i)-K(:,:,i)*c*PP(:,:,i);
        
    end
    
    P(:,:,1)=P(:,:,127);
    realX(:,1)=realX(:,127);
    estX(:,1)=estX(:,127);
    estXX(:,j+1)=estX(:,127);
    CP(:,:,j+1)=P(:,:,127);
    
end
for i=1:501
    
    est1(i)=estXX(1,i);
    est2(i)=estXX(2,i);
    est3(i)=estXX(3,i);
    est4(i)=estXX(4,i);
    est5(i)=estXX(5,i);
    est6(i)=estXX(6,i);
    est7(i)=estXX(7,i);
    est8(i)=estXX(8,i);
    est9(i)=estXX(9,i);
    est10(i)=estXX(10,i);
    est11(i)=estXX(11,i);
    est12(i)=estXX(12,i);
    est13(i)=estXX(13,i);
    est14(i)=estXX(14,i);
    est15(i)=estXX(15,i);
    
    PPP1(i)=CP(1,1,i);
    PPP2(i)=CP(2,2,i);
    PPP3(i)=CP(3,3,i);
    PPP4(i)=CP(4,4,i);
    PPP5(i)=CP(5,5,i);
    PPP6(i)=CP(6,6,i);
    PPP7(i)=CP(7,7,i);
    PPP8(i)=CP(8,8,i);
    PPP9(i)=CP(9,9,i);
    PPP10(i)=CP(10,10,i);
    PPP11(i)=CP(11,11,i);
    PPP12(i)=CP(12,12,i);
    PPP13(i)=CP(13,13,i);
    PPP14(i)=CP(14,14,i);
    PPP15(i)=CP(15,15,i);
    
end

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

t=0:1*252/3600:35;

figure(1)

subplot(3,1,1)
plot(t,est1,'b');
title('x1');
grid on;
subplot(3,1,2)
plot(t,est2,'b');
title('x2');
grid on;
subplot(3,1,3)
plot(t,est3,'b');
title('x3');
grid on;

figure(2)

subplot(3,1,1)
plot(t,est4,'b');
title('x4');
grid on;
subplot(3,1,2)
plot(t,est5,'b');
title('x5');
grid on;
subplot(3,1,3)
plot(t,est6,'b');
title('x6');
grid on;

figure(3)

subplot(3,1,1)
plot(t,est7,'b');
title('x7');
grid on;
subplot(3,1,2)
plot(t,est8,'b');
title('x8');
grid on;
subplot(3,1,3)
plot(t,est9,'b');
title('x9');
grid on;

figure(4)

subplot(3,1,1)
plot(t,est10,'b');
title('x10');
grid on;
subplot(3,1,2)
plot(t,est11,'b');
title('x11');
grid on;
subplot(3,1,3)
plot(t,est12,'b');
title('x12');
grid on;

figure(5)

subplot(3,1,1)
plot(t,est13,'b');
title('x13');
grid on;
subplot(3,1,2)
plot(t,est14,'b');
title('x14');
grid on;
subplot(3,1,3)
plot(t,est15,'b');
title('x15');
grid on;

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

figure(6)

subplot(3,1,1)
plot(t,PPP1,'b');
grid on;
subplot(3,1,2)
plot(t,PPP2,'b');
grid on;
subplot(3,1,3)
plot(t,PPP3,'b');
grid on;

figure(7)

subplot(3,1,1)
plot(t,PPP4,'b');
grid on;
subplot(3,1,2)
plot(t,PPP5,'b');
grid on;
subplot(3,1,3)
plot(t,PPP6,'b');
grid on;

figure(8)

subplot(3,1,1)
plot(t,PPP7,'b');
grid on;
subplot(3,1,2)
plot(t,PPP8,'b');
grid on;
subplot(3,1,3)
plot(t,PPP9,'b');
grid on;

figure(9)

subplot(3,1,1)
plot(t,PPP10,'b');
grid on;
subplot(3,1,2)
plot(t,PPP11,'b');
grid on;
subplot(3,1,3)
plot(t,PPP12,'b');
grid on; 

figure(10)

subplot(3,1,1)
plot(t,PPP13,'b');
grid on;
subplot(3,1,2)
plot(t,PPP14,'b');
grid on;
subplot(3,1,3)
plot(t,PPP15,'b');
grid on;