⭐ 欢迎来到虫虫下载站！ | 📦 资源下载 📁 资源专辑 ℹ️ 关于我们
⭐ 虫虫下载站
📤 上传资源

📄 wu_fangzhen1_kalman1_fujian3.m

📁 这些程序都是经过本人和许许多多工作过学习过的老师同学
💻 M
字号:
🔍 
%%%35个小时时的估计值
%%%%%%真实值%%%%%(采样时间为2秒）
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=zeros(12);

a=zeros(12,12,63001);

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

%系统的观测矩阵C阵

C=zeros(5,12);

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

%kalman滤波方程的定义

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

%量测噪声

v=[0.2 0.2 0.1*pi/180 0.1*pi/180 0.3*pi/180]';

RR=zeros(5);

for i=1:5
   
     RR(i,i)=v(i,1)^2;

end
                                             
%系统噪声

w=[0;0;0;0;0;0;0.01*pi/180;0.01*pi/180;0.01*pi/180;2e-4*g;2e-4*g;2e-4*g];

%系统噪声方差阵Q阵

Q=zeros(12);

for i=1:12
   
     Q(i,i)=w(i,1)^2*0.1;

end

%协方差矩阵的初始值

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 i=1:12
   
     P(i,i,1)=realX(i,1)^2;

end

%状态变量的初始值

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

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

for j=1:500
    
    for i=1:126
         
        PP(:,:,i+1)=a(:,:,i+1)*P(:,:,i)*a(:,:,i+1)'+Q;
        K(:,:,i+1)=PP(:,:,i+1)*C'*inv(C*PP(:,:,i+1)*C'+RR);
        realX(:,i+1)=a(:,:,i+1)*realX(:,i)+rand(1,12)*w;
        Z(:,i+1)=C*realX(:,i+1)+rand(1,5)*v;
        estX(:,i+1)=a(:,:,i+1)*estX(:,i)+K(:,:,i+1)*(Z(:,i+1)-C*(a(:,:,i+1)*estX(:,i)));
        P(:,:,i+1)=PP(:,:,i+1)-K(:,:,i+1)*C*PP(:,:,i+1);
        
    end
    a(:,:,1)=a(:,:,127);
    P(:,:,1)=P(:,:,127);
    realX(:,1)=realX(:,127);
    estX(:,1)=estX(:,127);
    estXX(:,j+1)=estX(:,127);
    realXX(:,j+1)=realX(:,127);
    CP(:,:,j+1)=P(:,:,127);
    
end

t=0:252/3600:35;


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);
    
    realX1(i)=realXX(1,i)*180/pi;
    realX2(i)=realXX(2,i)*180/pi;
    realX3(i)=realXX(3,i)*180/pi;
    realX4(i)=realXX(4,i);
    realX5(i)=realXX(5,i);
    realX6(i)=realXX(6,i)*180*60/pi;
    realX7(i)=realXX(7,i)*180/pi;
    realX8(i)=realXX(8,i)*180/pi;
    realX9(i)=realXX(9,i)*180/pi;
    realX10(i)=realXX(10,i)/(1e-6*g);
    realX11(i)=realXX(11,i)/(1e-6*g);
    realX12(i)=realXX(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(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;

figure(2)

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(3)

subplot(3,3,1)
plot(t,realX1,'b');
xlabel('time/h');
grid on;
subplot(3,3,2)
plot(t,realX2,'b');
xlabel('time/h');
grid on;
subplot(3,3,3)
plot(t,realX3,'b');
xlabel('time/h');
grid on;
subplot(3,3,4)
plot(t,realX4,'b');
xlabel('time/h');
grid on;
subplot(3,3,5)
plot(t,realX5,'b');
xlabel('time/h');
grid on;
subplot(3,3,6)
plot(t,realX6,'b');
xlabel('time/h');
grid on;

figure(4)

subplot(3,3,1)
plot(t,realX7,'b');
xlabel('time/h');
grid on;
subplot(3,3,2)
plot(t,realX8,'b');
xlabel('time/h');
grid on;
subplot(3,3,3)
plot(t,realX9,'b');
xlabel('time/h');
grid on;
subplot(3,3,4)
plot(t,realX10,'b');
xlabel('time/h');
grid on;
subplot(3,3,5)
plot(t,realX11,'b');
xlabel('time/h');
grid on;
subplot(3,3,6)
plot(t,realX12,'b');
xlabel('time/h');
grid on;

figure(5)

subplot(3,3,1)
plot(t,est1,'r',t,realX1,'b');
xlabel('time/h');
grid on;
subplot(3,3,2)
plot(t,est2,'r',t,realX2,'b');
xlabel('time/h');
grid on;
subplot(3,3,3)
plot(t,est3,'r',t,realX3,'b');
xlabel('time/h');
grid on;
subplot(3,3,4)
plot(t,est4,'r',t,realX4,'b');
xlabel('time/h');
grid on;
subplot(3,3,5)
plot(t,est5,'r',t,realX5,'b');
xlabel('time/h');
grid on;
subplot(3,3,6)
plot(t,est6,'r',t,realX6,'b');
xlabel('time/h');
grid on;


%协方差矩阵的仿真

figure(6)

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

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;


figure(8)

subplot(3,3,1)
plot(t,est1-realX1,'b');
xlabel('time/h');
grid on;
subplot(3,3,2)
plot(t,est2-realX2,'b');
xlabel('time/h');
grid on;
subplot(3,3,3)
plot(t,est3-realX3,'b');
xlabel('time/h');
grid on;
subplot(3,3,4)
plot(t,est4-realX4,'b');
xlabel('time/h');
grid on;
subplot(3,3,5)
plot(t,est5-realX5,'b');
xlabel('time/h');
grid on;
subplot(3,3,6)
plot(t,est6-realX6,'b');
xlabel('time/h');
grid on;

⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -