%***********加上x4,x5,x6后，且觀測值y1,y2,y3,y4,y5為已知，sWfx,sWfy,sWfz擴充為狀態變量的情況下可觀測性的仿真**********

clc
clear all

[t,y]=ode45('observability2c',[0:2:35*3600],[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]);

yy=zeros(501,12);

%每隔126點保存一次，放在矩陣yy

yy(1,:)=y(1,:);

for j=1:500
    
    for i=2:127
        
        yy(j+1,:)=y(126*j+1,:);
        
    end
    
end   

x1=yy(:,1)*180*60/pi;
x2=yy(:,2)*180*60/pi;
x3=yy(:,3)*180*60/pi;
x4=yy(:,4)*180*60/pi;
x5=yy(:,5)*180*60/pi;
x6=yy(:,6)*180*60/pi;
x7=yy(:,7);
x8=yy(:,8);
x9=yy(:,9)*180*60/pi;
x10=yy(:,10);
x11=yy(:,11);
x12=yy(:,12);

t=0:252/3600:35;

%艦體姿態誤差角

figure(1)

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

subplot(3,4,2)
plot(t,x2,'r');
xlabel('time/h');
grid on;

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

%彈體誤差角

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

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

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

%速度誤差

subplot(3,4,7)
plot(t,x7,'g');
xlabel('time/h');
grid on;

subplot(3,4,8)
plot(t,x1,'m');
xlabel('time/h');
grid on;

%經度誤差角

subplot(3,4,9)
plot(t,x9,'c');
xlabel('time/h');
grid on;

%擴充的狀態變量

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

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

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


%重力加速度

g=9.80; 

%地速分量

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

%地球半徑

R=6371020; 

%緯度角

angle=pi/4;

%地球自轉角速度

Wie=7.2921158e-5;  

%加速度計的漂移誤差

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);

%所測的比力信息

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;

%求出A陣

A=zeros(12);

A(1,2)= Vx*tan(angle)/R+Wie*sin(angle);
A(1,3)=-Vx/R-Wie*cos(angle);
A(1,8)=-1/R;
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(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(4,10)=1;
A(5,11)=1;
A(6,12)=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;

B=eye(12);

D=zeros(5,12);

%求出C陣

C=zeros(5,12);

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;

%離散化

[a,b,c,d]=c2dm(A,B,C,D,2);

%求出秩

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];
y=rank(E1);
fprintf('%d',y);
[u1,s1,v1]=svd(E1);
E2=[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];
[u,s,v]=svd(E2);

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

%系統的觀測噪聲的協方差強度陣

RR=zeros(5);

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

%置系統噪聲方差陣Q陣

Q=zeros(12);

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

%系統噪聲

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

%量測噪聲

v=[0.1*randn(1) 0.1*randn(1) pi/(60*180)*randn(1) pi/(60*180)*randn(1) 3*pi/(60*180)*randn(1)]';

%狀態變量的初始值

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]';

%協方差矩陣的初始值

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)=a*P2(:,:,i-1)*a'+Q;
     K2(:,:,i)=PP2(:,:,i)*c'*inv(c*PP2(:,:,i)*c'+RR);
     realX2(:,i)=a*realX2(:,i-1)+b*f2+w;
     Z2(:,i)=c*realX2(:,i)+v;
     estX2(:,i)=a*estX2(:,i-1)+b*f2+K2(:,:,i)*(Z2(:,i)-c*(a*estX2(:,i-1)+b*f2));
     P2(:,:,i)=PP2(:,:,i)-K2(:,:,i)*c*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(2)
subplot(3,1,1)
plot(t,est21,'r');
xlabel('time/h');
grid on;

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

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

figure(3)
subplot(3,1,1)
plot(t,est24,'r')
xlabel('time/h');
grid on;


subplot(3,1,2)
plot(t,est25,'r');
xlabel('time/h');
grid on;


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

figure(4)
subplot(3,1,1)
plot(t,est27,'r');
xlabel('time/h');
grid on;


subplot(3,1,2)
plot(t,est28,'r');
xlabel('time/h');
grid on;


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

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


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


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

figure(6)

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

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

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

figure(7)

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

subplot(3,1,2)
plot(t,est25'-x5,'b');
xlabel('time/h');
grid on;

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

figure(8)
subplot(3,1,1)
plot(t,est27'-x7,'b');
xlabel('time/h');
grid on;

subplot(3,1,2)
plot(t,est28'-x8,'b');
xlabel('time/h');
grid on;

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

figure(9)

subplot(3,1,1)
plot(t,est210'-x10,'b');
xlabel('time/h');
grid on;
subplot(3,1,2)
plot(t,est211'-x11,'b');
xlabel('time/h');
grid on;
subplot(3,1,3)
plot(t,est212'-x12,'b');
xlabel('time/h');
grid on;