?? wu_fangzhen1_kalman1_fujian4.m

?? 這些程序都是經過本人和許許多多工作過學習過的老師同學
?? M
字號:
%%仿真時間為21秒的估計值
%%%%%%%真實值
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,4);
realX=zeros(12,4);
PP=zeros(12,12,3);
P=zeros(12,12,4);
K=zeros(12,5,4);
Z=zeros(5,4);
estXX=zeros(12,71);
realXX=zeros(12,71);
CP=zeros(12,12,71);

%量測噪聲

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:70
    
    for i=1:3
         
        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(:,:,4);
    P(:,:,1)=P(:,:,4);
    realX(:,1)=realX(:,4);
    estX(:,1)=estX(:,4);
    estXX(:,j+1)=estX(:,4);
    realXX(:,j+1)=realX(:,4);
    CP(:,:,j+1)=P(:,:,4);
    
end

t=0:0.3:0.07*300;


for i=1:71
    
    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/s');
grid on;
subplot(3,3,2)
plot(t,est2,'b');
xlabel('time/s');
grid on;
subplot(3,3,3)
plot(t,est3,'b');
xlabel('time/s');
grid on;
subplot(3,3,4)
plot(t,est4,'b');
xlabel('time/s');
grid on;
subplot(3,3,5)
plot(t,est5,'b');
xlabel('time/s');
grid on;
subplot(3,3,6)
plot(t,est6,'b');
xlabel('time/s');
grid on;

figure(2)

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


figure(3)

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

figure(4)

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

figure(5)

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


%協方差矩陣的仿真

figure(6)

subplot(3,3,1)
plot(t,PPP1,'b');
xlabel('time/s');
grid on;
subplot(3,3,2)
plot(t,PPP2,'b');
xlabel('time/s');
grid on;
subplot(3,3,3)
plot(t,PPP3,'b');
xlabel('time/s');
grid on;
subplot(3,3,4)
plot(t,PPP4,'b');
xlabel('time/s');
grid on;
subplot(3,3,5)
plot(t,PPP5,'b');
xlabel('time/s');
grid on;
subplot(3,3,6)
plot(t,PPP6,'b');
xlabel('time/s');
grid on;

figure(7)

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


figure(8)

subplot(3,3,1)
plot(t,est1-realX1,'b');
xlabel('time/s');
grid on;
subplot(3,3,2)
plot(t,est2-realX2,'b');
xlabel('time/s');
grid on;
subplot(3,3,3)
plot(t,est3-realX3,'b');
xlabel('time/s');
grid on;
subplot(3,3,4)
plot(t,est4-realX4,'b');
xlabel('time/s');
grid on;
subplot(3,3,5)
plot(t,est5-realX5,'b');
xlabel('time/s');
grid on;
subplot(3,3,6)
plot(t,est6-realX6,'b');
xlabel('time/s');
grid on;
?? 文件大小 18 K
?? 上傳用戶 pengcong2008_
?? 所屬分類 matlab例程
??? 相關標簽

#程序
?? 快捷鍵說明

復制代碼 Ctrl + C
搜索代碼 Ctrl + F
全屏模式 F11
切換主題 Ctrl + Shift + D
顯示快捷鍵 ?
增大字號 Ctrl + =
減小字號 Ctrl + -
亚洲欧美第一页_禁久久精品乱码_粉嫩av一区二区三区免费野_久草精品视频

?? wu_fangzhen1_kalman1_fujian4.m

?? 快捷鍵說明
