?? jizaidaodan2.m
字號:
%************************************************************************%
%************************18階系統(tǒng)的秩和kalman濾波************************%
%************************************************************************%
clc
clear all;
%重力加速度
g=9.80;
%各軸向的地速分量
Vx=8.0;
Vy=8.0;
Vz=0.0;
%地球的半徑
R=6371020;
%當?shù)氐木暥冉?
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;
%系統(tǒng)的狀態(tài)矩陣A陣
A=zeros(18);
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)-Vz)/R;
A(7,8)=2*Wie*sin(angle)+Vx*tan(angle)/R;
A(7,9)=2*Wie*sin(angle)*Vz+(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)=-Vz/R;
A(8,9)=-2*Wie*cos(angle)*Vx-Vx^2*sec(angle)^2/R;
A(8,17)=1;
A(9,8)=1/R;
%系統(tǒng)的觀測矩陣C陣
C=zeros(3,18);
C(1,1)=1;
C(2,2)=1;
C(3,3)=1;
%系統(tǒng)方程離散化
B=zeros(18,1);
D=zeros(3,1);
[a,b,c,d]=c2dm(A,B,C,D,2);
%求出離散化后系統(tǒng)的秩
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;C*A^10;C*A^11;C*A^12;C*A^13;C*A^14;C*A^15;C*A^16;C*A^17];
[u,s,v]=svd(E);
y=rank(E);
fprintf('%d',y);
%kalman濾波方程的定義
estX=zeros(18,127);
realX=zeros(18,127);
PP=zeros(18,18,126);
P=zeros(18,18,127);
K=zeros(18,3,127);
Z=zeros(3,127);
estXX=zeros(18,501);
CP=zeros(18,18,501);
%系統(tǒng)的觀測噪聲的協(xié)方差強度陣
RR=zeros(3);
RR(1,1)=0.01;
RR(2,2)=0.01;
RR(3,3)=0.01;
%系統(tǒng)噪聲方差陣Q陣
Q=zeros(18);
Q(1,1)=(1e-5*g)^2;
Q(2,2)=(1e-5*g)^2;
Q(3,3)=(1e-5*g)^2;
Q(4,4)=(0.01*pi/(180*3600))^2;
Q(5,5)=(0.01*pi/(180*3600))^2;
Q(6,6)=(0.01*pi/(180*3600))^2;
%系統(tǒng)噪聲
w=[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];
%量測噪聲
v=[0.1*randn(1) 0.1*randn(1) 0.1*randn(1)]';
%狀態(tài)變量的初始值
estX(:,1)=[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]';
realX(:,1)=[0.1 0.1 0.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) 0 0 0 1e-4*g 1e-4*g 0 0 0]';
%協(xié)方差矩陣的初始值
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)=(5*pi/(60*180))^2;
P(5,5,1)=(5*pi/(60*180))^2;
P(6,6,1)=(6*pi/(60*180))^2;
P(7,7,1)=0.01;
P(8,8,1)=0.01;
P(9,9,1)=(2*pi/(60*180))^2;
P(10,10,1)=(0.1*pi/(180*3600))^2;
P(11,11,1)=(0.1*pi/(180*3600))^2;
P(12,12,1)=(0.1*pi/(180*3600))^2;
P(13,13,1)=0.01;
P(14,14,1)=0.125^2;
P(15,15,1)=0.001^2;
P(16,16,1)=((1e-4)*g)^2;
P(17,17,1)=((1e-4)*g)^2;]
P(18,18,1)=((1e-4)*g)^2;
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;
est11(i)=estXX(11,i)*180*3600/pi;
est12(i)=estXX(12,i)*180*3600/pi;
est13(i)=estXX(13,i);
est14(i)=estXX(14,i);
est15(i)=estXX(15,i);
est16(i)=estXX(16,i)/(1e-6*g);
est17(i)=estXX(17,i)/(1e-6*g);
est18(i)=estXX(18,i)/(1e-6*g);
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;
PPP11(i)=CP(11,11,i)*(180*3600/pi)^2;
PPP12(i)=CP(12,12,i)*(180*3600/pi)^2;
PPP13(i)=CP(13,13,i);
PPP14(i)=CP(14,14,i);
PPP15(i)=CP(15,15,i);
PPP16(i)=CP(16,16,i)/(1e-6*g)^2;
PPP17(i)=CP(17,17,i)/(1e-6*g)^2;
PPP18(i)=CP(18,18,i)/(1e-6*g)^2;
end
%畫出狀態(tài)變量的估計值的曲線圖
t=0:252/3600:35;
figure(1)
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(2)
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(3)
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(4)
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(5)
subplot(3,1,1)
plot(t,est13,'b');
xlabel('time/h');
grid on;
subplot(3,1,2)
plot(t,est14,'b');
xlabel('time/h');
grid on;
subplot(3,1,3)
plot(t,est15,'b');
xlabel('time/h');
grid on;
figure(6)
subplot(3,1,1)
plot(t,est16,'b');
xlabel('time/h');
grid on;
subplot(3,1,2)
plot(t,est17,'b');
xlabel('time/h');
grid on;
subplot(3,1,3)
plot(t,est18,'b');
xlabel('time/h');
grid on;
%協(xié)方差矩陣的仿真
figure(7)
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(8)
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(9)
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(10)
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(11)
subplot(3,1,1)
plot(t,PPP13,'b');
xlabel('time/h');
grid on;
subplot(3,1,2)
plot(t,PPP14,'b');
xlabel('time/h');
grid on;
subplot(3,1,3)
plot(t,PPP15,'b');
xlabel('time/h');
grid on;
figure(12)
subplot(3,1,1)
plot(t,PPP16,'b');
xlabel('time/h');
grid on;
subplot(3,1,2)
plot(t,PPP17,'b');
xlabel('time/h');
grid on;
subplot(3,1,3)
plot(t,PPP18,'b');
xlabel('time/h');
grid on;
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -