wu_jianyan_chengxuzhengquexing.m

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

clc;
clear all;

fid1= fopen('E:\孙国伟\work\data51.txt','r');
data1=fscanf(fid1,'%f',[3,63001]);

fid2= fopen('E:\孙国伟\work\data52.txt','r');
data2=fscanf(fid2,'%f',[3,63001]);

fid3= fopen('E:\孙国伟\work\data53.txt','r');
data3=fscanf(fid3,'%f',[3,63001]);

T=zeros(3,3,63001);

T(1,:,:)=data1;
T(2,:,:)=data2;
T(3,:,:)=data3;

%重力加速度

g=9.80; 

%地速分量

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

%地球半径

R=6371020; 

%纬度角

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;

%求出A阵

A=zeros(6);

A(1,2)= Vx*tan(angle)/R+Wie*sin(angle);
A(1,3)=-Vx/R-Wie*cos(angle);
A(1,5)=-1/R;
A(2,1)=-Vx*tan(angle)/R-Wie*sin(angle);
A(2,3)=-Vy/R;
A(2,4)=1/R;
A(2,6)=-Wie*sin(angle);
A(3,1)=Vx/R+Wie*cos(angle);
A(3,2)=Vy/R;
A(3,4)=tan(angle)/R;
A(3,6)=Wie*cos(angle)+Vx*sec(angle)^2/R;
A(4,2)=-fz;
A(4,3)=fy; 
A(4,4)=(Vy*tan(angle)-Vz)/R;
A(4,5)=2*Wie*sin(angle)+Vx*tan(angle)/R; 
A(4,6)=2*Wie*sin(angle)*Vz+(2*Wie*cos(angle)+Vx*sec(angle)^2/R)*Vy;
A(5,1)=fz;
A(5,3)=-fx;
A(5,4)=-2*Wie*sin(angle)-2*Vx*tan(angle)/R;
A(5,5)=-Vz/R;
A(5,6)=-2*Wie*cos(angle)*Vx-Vx^2*sec(angle)^2/R;
A(6,5)=1/R;

A1=zeros(12,12,63001);

for i=1:63001

A1(1,2,i)=A(1,2);
A1(1,3,i)=A(1,3);
A1(1,5,i)=A(1,5);
A1(1,7,i)=T(1,1,i);
A1(1,8,i)=T(1,2,i);
A1(1,9,i)=T(1,3,i);

A1(2,1,i)=A(2,1);
A1(2,3,i)=A(2,3);
A1(2,4,i)=A(2,4);
A1(2,6,i)=A(2,6);
A1(2,7,i)=T(2,1,i);
A1(2,8,i)=T(2,2,i);
A1(2,9,i)=T(2,3,i);

A1(3,1,i)=A(3,1);
A1(3,2,i)=A(3,2);
A1(3,4,i)=A(3,4);
A1(3,6,i)=A(3,6);
A1(3,7,i)=T(3,1,i);
A1(3,8,i)=T(3,2,i);
A1(3,9,i)=T(3,3,i);

A1(4,2,i)=A(4,2);
A1(4,3,i)=A(4,3); 
A1(4,4,i)=A(4,4);
A1(4,5,i)=A(4,5); 
A1(4,6,i)=A(4,6);
A1(4,10,i)=T(1,1,i);
A1(4,11,i)=T(1,2,i);
A1(4,12,i)=T(1,3,i);

A1(5,1,i)=A(5,1);
A1(5,3,i)=A(5,3);
A1(5,4,i)=A(5,4);
A1(5,5,i)=A(5,5);
A1(5,6,i)=A(5,6);
A1(5,10,i)=T(2,1,i);
A1(5,11,i)=T(2,2,i);
A1(5,12,i)=T(2,3,i);

A1(6,5,i)=A(6,5);

end

B=eye(12);

a=zeros(12,12,63001);

for i=1:63001
    
    [a(:,:,i),b]=c2d(A1(:,:,i),B,2);
    
end

%系统的观测矩阵C阵

C=zeros(3,12);

C(1,4)=1;
C(2,5)=1;
C(3,3)=1;

%kalman滤波方程的定义

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

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

realX(:,1)=[0.1*pi/180 0.1*pi/180 0.3*pi/180 0.2 0.2 2*pi/(60*180) 3.01*pi/180 3.01*pi/180 3.01*pi/180 3.8e-3*g 3.8e-3*g 3.8e-3*g]';

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

%协方差矩阵的初始值

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

%状态变量的初始值

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

realX(:,1)=[0.1*pi/180 0.1*pi/180 0.3*pi/180 0.2 0.2 2*pi/(60*180) 3.01*pi/180 3.01*pi/180 3.01*pi/180 3.8e-3*g 3.8e-3*g 3.8e-3*g]';

for j=1:500
    
    for i=1:126
        
        a(:,:,i)=a(:,:,126*(j-1)+i);
        PP(:,:,i+1)=a(:,:,i)*P(:,:,i)*a(:,:,i)';
        K(:,:,i+1)=PP(:,:,i+1)*C'*inv(C*PP(:,:,i+1)*C');
        realX(:,i+1)=a(:,:,i)*realX(:,i);
        Z(:,i+1)=C*realX(:,i+1);
        estX(:,i+1)=a(:,:,i)*estX(:,i)+K(:,:,i+1)*(Z(:,i+1)-C*(a(:,:,i)*estX(:,i)));
        P(:,:,i+1)=PP(:,:,i+1)-K(:,:,i+1)*C*PP(:,:,i+1);
        
    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/pi;
    est2(i)=estXX(2,i)*180/pi;
    est3(i)=estXX(3,i)*180/pi;
    est4(i)=estXX(4,i);
    est5(i)=estXX(5,i);
    est6(i)=estXX(6,i)*180*60/pi;
    est7(i)=estXX(7,i)*180/pi;
    est8(i)=estXX(8,i)*180/pi;
    est9(i)=estXX(9,i)*180/pi;
    est10(i)=estXX(10,i)/(1e-6*g);
    est11(i)=estXX(11,i)/(1e-6*g);
    est12(i)=estXX(12,i)/(1e-6*g);
    
    PPP1(i)=CP(1,1,i)*(180/pi)^2;
    PPP2(i)=CP(2,2,i)*(180/pi)^2;
    PPP3(i)=CP(3,3,i)*(180/pi)^2;
    PPP4(i)=CP(4,4,i);
    PPP5(i)=CP(5,5,i);
    PPP6(i)=CP(6,6,i)*(180*60/pi)^2;
    PPP7(i)=CP(7,7,i)*(180/pi)^2;
    PPP8(i)=CP(8,8,i)*(180/pi)^2;
    PPP9(i)=CP(9,9,i)*(180/pi)^2;
    PPP10(i)=CP(10,10,i)/(1e-6*g)^2;
    PPP11(i)=CP(11,11,i)/(1e-6*g)^2;
    PPP12(i)=CP(12,12,i)/(1e-6*g)^2;
    
end

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


figure(3)

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;

figure(4)

subplot(3,3,1)
plot(t,est7,'b');
xlabel('time/h');
grid on;
subplot(3,3,2)
plot(t,est8,'b');
xlabel('time/h');
grid on;
subplot(3,3,3)
plot(t,est9,'b');
xlabel('time/h');
grid on;
subplot(3,3,4)
plot(t,est10,'b');
xlabel('time/h');
grid on;
subplot(3,3,5)
plot(t,est11,'b');
xlabel('time/h');
grid on;
subplot(3,3,6)
plot(t,est12,'b');
xlabel('time/h');
grid on;

%协方差矩阵的仿真

figure(5)

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;

figure(6)

subplot(3,3,1)
plot(t,PPP7,'b');
xlabel('time/h');
grid on;
subplot(3,3,2)
plot(t,PPP8,'b');
xlabel('time/h');
grid on;
subplot(3,3,3)
plot(t,PPP9,'b');
xlabel('time/h');
grid on;
subplot(3,3,4)
plot(t,PPP10,'b');
xlabel('time/h');
grid on;
subplot(3,3,5)
plot(t,PPP11,'b');
xlabel('time/h');
grid on;
subplot(3,3,6)
plot(t,PPP12,'b');
xlabel('time/h');
grid on;