%***************************系統1a**********************

clc
clear all;

format long;

%重力加速度

g=9.80; 

%緯度角

angle=32*pi/180;

%地球自轉角速度

Wie=7.2921158e-5;  

%求出A陣

A=zeros(8);

A(1,2)=Wie*sin(angle);
A(1,4)=1;
A(2,1)=-Wie*sin(angle);
A(2,3)=Wie*cos(angle);
A(2,6)=1;
A(3,2)=-Wie*cos(angle);
A(3,5)=1;

%求出C陣

C=zeros(2,8);

C(1,2)=g;
C(1,6)=1;
C(2,1)=-g;
C(2,8)=1;

B=zeros(8,3);

B(1,1)=1;
B(2,2)=1;
B(3,3)=1;

Q=[C;C*A;C*A^2;C*A^3;C*A^4;C*A^5;C*A^6;C*A^7];

[V,D]=eig(Q'*Q);

D1=D/D(8,8);

[u,s,v]=svd(Q);

%觀測值

X(:,1)=[3*pi/(60*180);3*pi/(60*180);5*pi/(60*180);0.01*pi/(180*3600);0.01*pi/(180*3600);0.01*pi/(180*3600);1e-4*g;1e-4*g];

for i=1:8
    
    Y(:,:,i)=C*X(:,i);
    X(:,i+1)=A*X(:,i);
    
end


Y=[Y(:,:,1);Y(:,:,2);Y(:,:,3);Y(:,:,4);Y(:,:,5);Y(:,:,6);Y(:,:,7);Y(:,:,8)];

x01=u(:,1)'*Y*v(:,1)/s(1,1);
x02=u(:,2)'*Y*v(:,2)/s(2,2);
x03=u(:,3)'*Y*v(:,3)/s(3,3);
x04=u(:,4)'*Y*v(:,4)/s(4,4);
x05=u(:,5)'*Y*v(:,5)/s(5,5);
x06=u(:,6)'*Y*v(:,6)/s(6,6);
x07=u(:,7)'*Y*v(:,7)/s(7,7);
x08=u(:,8)'*Y*v(:,8)/s(8,8);

%畫直方圖

figure(1)

subplot(3,2,1)
bar(x01);
subplot(3,2,2)
bar(x02);
subplot(3,2,3)
bar(x03);
subplot(3,2,4)
bar(x04);
subplot(3,2,5)
bar(x05);
subplot(3,2,6)
bar(x06);

figure(2)

subplot(3,2,1)
bar(x07);
subplot(3,2,2)
bar(x08);