jizaidaodan1.m

这些程序都是经过本人和许许多多工作过学习过的老师同学

clc;
clear all;

format long;

%重力加速度

g=9.80;    

%地球半径

R=6371020;

%纬度角

angle=pi/4; 

%地球自转角速度

Wie=7.2921158e-5;

%求出A阵

A=zeros(10);

A(1,2)=Wie*sin(angle);
A(1,3)=-Wie*cos(angle);
A(1,4)=1;
A(1,10)=-1/R;
A(2,1)=-Wie*sin(angle);
A(2,5)=1;
A(2,9)=1/R;
A(3,1)=Wie*cos(angle);
A(3,6)=1;
A(3,9)=tan(angle)/R;
A(9,2)=-g;
A(9,7)=1;
A(10,1)=g;
A(10,8)=1;

%求出C阵

C=zeros(2,10);

C(1,9)=1;
C(2,10)=1;

%系统方程离散化

B=zeros(10,1);

D=zeros(2,1);

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

%求出秩

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

for i=1:10
    
    fprintf('%20.10f\n',s(i,i));
    
end

%观测值

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

for i=1:10
    
    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);Y(:,:,9);Y(:,:,10)];

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);
x09=u(:,9)'*Y*v(:,9)/s(9,9);
x010=u(:,10)'*Y*v(:,10)/s(10,10);


figure(5)

subplot(3,3,1)
bar(x01);
subplot(3,3,2)
bar(x02);
subplot(3,3,3)
bar(x03);
subplot(3,3,4)
bar(x04);
subplot(3,3,5)
bar(x05);
subplot(3,3,6)
bar(x06);
subplot(3,3,7)
bar(x07);
subplot(3,3,8)
bar(x08);
subplot(3,3,9)
bar(x09);

figure(6)

subplot(3,3,1)
bar(x010);


%kalman滤波方程的定义

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

%系统的观测噪声的协方差强度阵

RR=zeros(2);

RR(1,1)=0.01;
RR(2,2)=0.01;

%系统噪声方差阵Q阵

Q=zeros(10);

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(9,9)=(1e-5*g)^2;              
Q(10,10)=(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;0;0;0;0;(1e-5*g)*randn(1);(1e-5*g)*randn(1)];

%量测噪声

v=[0.1*randn(1) 0.1*randn(1)]';

%状态变量的初始值

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

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

%协方差矩阵的初始值

P(1,1,1)=(3*pi/(60*180))^2;
P(2,2,1)=(3*pi/(60*180))^2;
P(3,3,1)=(5*pi/(60*180))^2;
P(4,4,1)=(0.1*pi/(180*3600))^2;
P(5,5,1)=(0.1*pi/(180*3600))^2;
P(6,6,1)=(0.1*pi/(180*3600))^2;
P(7,7,1)=(1e-4*g)^2;
P(8,8,1)=(1e-4*g)^2;
P(9,9,1)=0.01;
P(10,10,1)=0.01;

CP(:,:,1)=P(:,:,1);

for j=1:500
    
    for i=2:127
        
        PP(:,:,i)=a*P(:,:,i-1)*a'+Q;
        K(:,:,i)=PP(:,:,i)*c'*inv(c*PP(:,:,i)*c'+RR);
        realX(:,i)=a*realX(:,i-1)+w;
        Z(:,i)=c*realX(:,i)+v;
        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)*180*60/pi;
    est2(i)=estXX(2,i)*180*60/pi;
    est3(i)=estXX(3,i)*180*60/pi;
    est4(i)=estXX(4,i)*180*60/pi;
    est5(i)=estXX(5,i)*180*60/pi;
    est6(i)=estXX(6,i)*180*60/pi;
    est7(i)=estXX(7,i);
    est8(i)=estXX(8,i);
    est9(i)=estXX(9,i)*180*60/pi;
    est10(i)=estXX(10,i)*180*3600/pi;
    
    
    PPP1(i)=CP(1,1,i)*(180*60/pi)^2;
    PPP2(i)=CP(2,2,i)*(180*60/pi)^2;
    PPP3(i)=CP(3,3,i)*(180*60/pi)^2;
    PPP4(i)=CP(4,4,i)*(180*60/pi)^2;
    PPP5(i)=CP(5,5,i)*(180*60/pi)^2;
    PPP6(i)=CP(6,6,i)*(180*60/pi)^2;
    PPP7(i)=CP(7,7,i);
    PPP8(i)=CP(8,8,i);
    PPP9(i)=CP(9,9,i)*(180*60/pi)^2;
    PPP10(i)=CP(10,10,i)*(180*3600/pi)^2;
    
end

%画出状态变量的估计值的曲线图

t=0:252/3600:35;

figure(1)

subplot(3,3,1)
plot(t,est1,'b');
xlabel('time/h');
grid on;
subplot(3,3,2)
plot(t,est2,'b');
xlabel('time/h');
grid on;
subplot(3,3,3)
plot(t,est3,'b');
xlabel('time/h');
grid on;
subplot(3,3,4)
plot(t,est4,'b');
xlabel('time/h');
grid on;
subplot(3,3,5)
plot(t,est5,'b');
xlabel('time/h');
grid on;
subplot(3,3,6)
plot(t,est6,'b');
xlabel('time/h');
grid on;
subplot(3,3,7)
plot(t,est7,'b');
xlabel('time/h');
grid on;
subplot(3,3,8)
plot(t,est8,'b');
xlabel('time/h');
grid on;
subplot(3,3,9)
plot(t,est9,'b');
xlabel('time/h');
grid on;

figure(2)

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



%协方差矩阵的仿真

figure(3)

subplot(3,3,1)
plot(t,PPP1,'b');
xlabel('time/h');
grid on;
subplot(3,3,2)
plot(t,PPP2,'b');
xlabel('time/h');
grid on;
subplot(3,3,3)
plot(t,PPP3,'b');
xlabel('time/h');
grid on;
subplot(3,3,4)
plot(t,PPP4,'b');
xlabel('time/h');
grid on;
subplot(3,3,5)
plot(t,PPP5,'b');
xlabel('time/h');
grid on;
subplot(3,3,6)
plot(t,PPP6,'b');
xlabel('time/h');
grid on;
subplot(3,3,7)
plot(t,PPP7,'b');
xlabel('time/h');
grid on;
subplot(3,3,8)
plot(t,PPP8,'b');
xlabel('time/h');
grid on;
subplot(3,3,9)
plot(t,PPP9,'b');
xlabel('time/h');
grid on;

figure(4)

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