📄 yunxing2e.m

📁 这些程序都是经过本人和许许多多工作过学习过的老师同学
💻 M
字号:
%************************************************************%
%************13个状态变量及其线性组合时的仿真程序************%
%************************************************************%

clc
clear all

[t,y]=ode45('observability2e',[0:2:35*3600],[0.1;0.1;2*pi/(60*180);2*pi/(60*180);pi/(60*180);-0.0075591;0.0095215;1.0161e-6;1.0318e-6;8.7528e-7;6.3446e-6;-4.4075e-6;1.5028e-9]);

yy=zeros(501,13);

%每隔126点保存一次，放在矩阵yy

yy(1,:)=y(1,:);

for j=1:500
    
    for i=2:127
        
        yy(j+1,:)=y(126*j+1,:);
        
    end
    
end   

x1=yy(:,1);
x2=yy(:,2);
x3=yy(:,3)*180*60/pi;
x4=yy(:,4)*180*60/pi;
x5=yy(:,5)*180*60/pi;
x6=yy(:,6);
x7=yy(:,7);
x8=yy(:,8);
x9=yy(:,9);
x10=yy(:,10);
x11=yy(:,11);
x12=yy(:,12);
x13=yy(:,13);


%画图

t=0:252/3600:35;

figure(1)

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

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

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

subplot(2,3,4)
plot(t,x4,'b');
xlabel('time/h');
grid on;

subplot(2,3,5)
plot(t,x5,'b');
xlabel('time/h');
grid on;

subplot(2,3,6)
plot(t,x6,'b');
xlabel('time/h');
grid on;

figure(2)

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

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

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

subplot(2,3,4)
plot(t,x10,'b');
xlabel('time/h');
grid on;

subplot(2,3,5)
plot(t,x11,'b');
xlabel('time/h');
grid on;

subplot(2,3,6)
plot(t,x12,'b');
xlabel('time/h');
grid on;

figure(3)

plot(t,x13,'b');
xlabel('time/h');
grid on;


%重力加速度

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

A(1,2)= Vx*tan(angle)/R+Wie*sin(angle);
A(1,3)=-Vx/R-Wie*cos(angle);
A(1,8)=-1/R;
A(1,10)=-1;
A(2,1)=-Vx*tan(angle)/R-Wie*sin(angle);
A(2,3)=-Vy/R;
A(2,7)=1/R;
A(2,9)=-Wie*sin(angle);
A(2,11)=-1;
A(3,1)=Vx/R+Wie*cos(angle);
A(3,2)=Vy/R;
A(3,7)=tan(angle)/R;
A(3,9)=Wie*cos(angle)+Vx*sec(angle)^2/R;
A(3,12)=-1;
A(4,10)=1;
A(4,13)=1;
A(5,11)=1;
A(5,14)=1;
A(6,12)=1;
A(6,15)=1;
A(7,2)=-fz;
A(7,3)=fy; 
A(7,7)=Vy*tan(angle)/R;
A(7,8)=2*Wie*sin(angle)+Vx*tan(angle)/R; 
A(7,9)=(2*Wie*cos(angle)+Vx*sec(angle)^2/R)*Vy;
A(7,16)=1;
A(8,1)=fz;
A(8,3)=-fx;
A(8,7)=-2*Wie*sin(angle)-2*Vx*tan(angle)/R;
A(8,8)=0;
A(8,9)=-2*Wie*cos(angle)*Vx-Vx^2*sec(angle)^2/R;
A(8,17)=1;
A(9,8)=1/R;

%系统的观测矩阵C阵

C=zeros(5,17);

C(1,7)=1;
C(2,8)=1;
C(3,1)=-1;
C(3,4)=1;
C(4,2)=-1;
C(4,5)=1;
C(5,3)=-1;
C(5,6)=1;

%选加的4个线性无关的行

D=zeros(4,17);

D(1,1)=1;
D(2,2)=1;
D(3,3)=1;
D(4,17)=1;

%构造能观测性结构分解的变换阵T

E1=[C*A^2];

T=[C;C*A;E1([1 2 3],:);D];

r1=rank(T);

%线性定常系统按能观性结构分解

a=T*A*inv(T);
c=C*inv(T);
aa=a([1 2 3 4 5 6 7 8 9 10 11 12 13],[1 2 3 4 5 6 7 8 9 10 11 12 13]);
cc=c(:,[1 2 3 4 5 6 7 8 9 10 11 12 13]);

%能观性分解后系统的秩

E2=[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;c*a^10;c*a^11;c*a^12;c*a^13;c*a^14;c*a^15;c*a^16];

r2=rank(E2);

%系统方程离散化

bb=zeros(13,1);

dd=zeros(5,1);

[a1,b1,c1,d1]=c2dm(aa,bb,cc,dd,2);

%离散化后系统的秩

E3=[c1;c1*a1;c1*a1^2;c1*a1^3;c1*a1^4;c1*a1^5;c1*a1^6;c1*a1^7;c1*a1^8;c1*a1^9;c1*a1^10;c1*a1^11;c1*a1^12;c1*a1^13;c1*a1^14;c1*a1^15;c1*a1^16];

r3=rank(E3);

%kalman滤波方程的定义

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

X=[3*pi/(60*180);3*pi/(60*180);5*pi/(60*180);5*pi/(60*180);5*pi/(60*180);6*pi/(60*180);0.1;0.1;2*pi/(60*180);0.1*pi/(180*3600);0.1*pi/(180*3600); 0.1*pi/(180*3600);0;0;0;1e-4*g;1e-4*g];
T1=T([1 2 3 4 5 6 7 8 9 10 11 12 13],:);
XX=T1*X;

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

RR=zeros(5);

RR(1,1)=0.01;
RR(2,2)=0.01;
RR(3,3)=(pi/(60*180))^2;
RR(4,4)=(pi/(60*180))^2;
RR(5,5)=(3*pi/(60*180))^2;

%系统噪声

w1=[0.01*pi/(180*3600);0.01*pi/(180*3600);0.01*pi/(180*3600);0.01*pi/(180*3600);0.01*pi/(180*3600);0.01*pi/(180*3600);(1e-5*g);(1e-5*g);0;0;0;0;0;0;0;0;0];

w2=[0.01*pi/(180*3600)*randn(1);0.01*pi/(180*3600)*randn(1);0.01*pi/(180*3600)*randn(1);0.01*pi/(180*3600)*randn(1);0.01*pi/(180*3600)*randn(1);0.01*pi/(180*3600)*randn(1);(1e-5*g)*randn(1);(1e-5*g)*randn(1);0;0;0;0;0;0;0;0;0];

w=T1*w1;

w3=T1*w2;
%系统噪声方差阵Q阵

Q=zeros(13);

Q(1,1)=w(1,1)^2;
Q(2,2)=w(2,1)^2;
Q(3,3)=w(3,1)^2;
Q(4,4)=w(4,1)^2; 
Q(5,5)=w(5,1)^2;
Q(6,6)=w(6,1)^2;
Q(7,7)=w(7,1)^2;
Q(8,8)=w(8,1)^2;
Q(9,9)=w(9,1)^2;
Q(10,10)=w(10,1)^2;
Q(11,11)=w(11,1)^2;
Q(12,12)=w(12,1)^2;
Q(13,13)=w(13,1)^2;

%量测噪声 

v=[0.1*randn(1) 0.1*randn(1) pi/(60*180)*randn(1) pi/(60*180)*randn(1) 3*pi/(60*180)*randn(1)]';


%状态变量的初始估计值

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

realX(:,1)=XX;

%协方差矩阵的初始值

P(1,1,1)=XX(1,1)^2;
P(2,2,1)=XX(2,1)^2;
P(3,3,1)=XX(3,1)^2;
P(4,4,1)=XX(4,1)^2;
P(5,5,1)=XX(5,1)^2;
P(6,6,1)=XX(6,1)^2;
P(7,7,1)=XX(7,1)^2;
P(8,8,1)=XX(8,1)^2;
P(9,9,1)=XX(9,1)^2;
P(10,10,1)=XX(10,1)^2;
P(11,11,1)=XX(11,1)^2;
P(12,12,1)=XX(12,1)^2;
P(13,13,1)=XX(13,1)^2;

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

for j=1:500
    
    for i=2:127
        
        PP(:,:,i)=a1*P(:,:,i-1)*a1'+Q;
        K(:,:,i)=PP(:,:,i)*c1'*inv(c1*PP(:,:,i)*c1'+RR);
        realX(:,i)=a1*realX(:,i-1)+w3;
        Z(:,i)=c1*realX(:,i)+v;
        estX(:,i)=a1*estX(:,i-1)+K(:,:,i)*(Z(:,i)-c1*a1*estX(:,i-1));
        P(:,:,i)=PP(:,:,i)-K(:,:,i)*c1*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);
    est2(i)=estXX(2,i);
    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);
    est7(i)=estXX(7,i);
    est8(i)=estXX(8,i);
    est9(i)=estXX(9,i);
    est10(i)=estXX(10,i);
    est11(i)=estXX(11,i);
    est12(i)=estXX(12,i);
    est13(i)=estXX(13,i);
    
    PPP1(i)=CP(1,1,i);
    PPP2(i)=CP(2,2,i);
    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);
    PPP7(i)=CP(7,7,i);
    PPP8(i)=CP(8,8,i);
    PPP9(i)=CP(9,9,i);
    PPP10(i)=CP(10,10,i);
    PPP11(i)=CP(11,11,i);
    PPP12(i)=CP(12,12,i);
    PPP13(i)=CP(13,13,i);
     
end

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

t=0:252/3600:35;

figure(4)

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

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

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

figure(5)

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

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

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

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

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

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

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

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

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

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

%协方差矩阵

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


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


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

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


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


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

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

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


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

figure(12)

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


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


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

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

figure(14)

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

subplot(3,1,2)
plot(t,est2'-x2,'b');
xlabel('time/h');
grid on;

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

figure(15)

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

subplot(3,1,2)
plot(t,est5'-x5,'b');
xlabel('time/h');
grid on;

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

figure(16)
subplot(3,1,1)
plot(t,est7'-x7,'b');
xlabel('time/h');
grid on;

subplot(3,1,2)
plot(t,est8'-x8,'b');
xlabel('time/h');
grid on;

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

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

subplot(3,1,2)
plot(t,est11'-x11,'b');
xlabel('time/h');
grid on;

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

figure(18)
subplot(3,1,1)
plot(t,est13'-x13,'b');
xlabel('time/h');
grid on;
💿 文件大小 14 K
👤 上传用户 dasdasdc
📂 所属分类 matlab例程
🏷️ 相关标签

#程序
⌨️ 快捷键说明

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