📄 modulated10_1.m
字号:
%************************** psi角方程 **********
% kalman filter
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear;
clc;
maxcount=60*8;
Ts=1;
f1=fopen('zhuanjiao.txt');
s1=fscanf(f1,' %e',inf);
fclose all;
temp=((s1/12.6-0.01)*180/pi-0.1139);
for j=1:length(s1)
if temp(j)>45.4 ; temp(j)=45; end
if temp(j)<44.6 ; temp(j)=45; end
end
axis([0,544,-0.3,0.3]);
plot(0.5*(temp-45)/10*60);xlabel('时间/秒');ylabel('转动速度误差/角分');grid;%hold on;plot(45*ones(544,1),'r*')
s=zeros(10000,1);
s=s1;
s((length(s1)+1):(2*length(s1)))=s1;
s((length(s)+1):(2*length(s)))=s;
s((length(s)+1):(2*length(s)))=s;
s((length(s)+1):(2*length(s)))=s;
s((length(s)+1):(2*length(s)))=s;
w1=zeros(900,1);
for j=1:60*15
for i=1:10
w1(j)=s(10*(j-1)+i)+w1(j);
end
end
%w= 45*pi/180;%弧度/秒
w=100.47;
%-----------------------------------------
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
PP=[0.1^2 0 0 0 0 0 0 0 0 0 ;
0 0.1^2 0 0 0 0 0 0 0 0 ;
0 0 (1*pi/180)^2 0 0 0 0 0 0 0 ;
0 0 0 (1*pi/180)^2 0 0 0 0 0 0 ;
0 0 0 0 (1*pi/180)^2 0 0 0 0 0 ;
0 0 0 0 0 (0.0001*9.8)^2 0 0 0 0 ;
0 0 0 0 0 0 (0.0001*9.8)^2 0 0 0 ;
0 0 0 0 0 0 0 (0.3*pi/180/3600)^2 0 0 ;
0 0 0 0 0 0 0 0 (0.3*pi/180/3600)^2 0 ;
0 0 0 0 0 0 0 0 0 (0.3*pi/180/3600)^2 ];
Q=diag([(0.00005*9.8)^2, (0.00005*9.8)^2, (1*pi/180/3600)^2, (1*pi/180/3600)^2, (1*pi/180/3600)^2, 0 , 0 , 0 , 0 ,0],0);
R=diag([0.1*0.1,0.1*0.1],0);
H=[1 0 0 0 0 0 0 0 0 0;
0 1 0 0 0 0 0 0 0 0];
Ww=360.0*pi/24/180/3600; %单位是弧度-
L=40.0;
WD=-Ww*sin(L*pi/180);
WN= Ww*cos(L*pi/180);
g=9.80;
B=eye(10);
%*****************************************************
cbs = [cos(w*1*Ts) sin(w*1*Ts) 0;
-sin(w*1*Ts) cos(w*1*Ts) 0;
0 0 1];
csb=cbs';
A=[0 2*WD 0 g 0 csb(1,1) csb(1,2) 0 0 0;
-2*WD 0 -g 0 0 csb(2,1) csb(2,2) 0 0 0;
0 0 0 WD 0 0 0 csb(1,1) csb(1,2) csb(1,3);
0 0 -WD 0 WN 0 0 csb(2,1) csb(2,2) csb(2,3);
0 0 0 -WN 0 0 0 csb(3,1) csb(3,2) csb(3,3);
0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0];
sys=ss(A,B,H,0);
sysd=c2d(sys,Ts);
[F1,G1,C1,Dd]=ssdata(sysd);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%55
V=zeros( 2,maxcount);
W=zeros(10,maxcount);
for i=1:10
W(i,:)=sqrt(Q(i,i))*randn(1,maxcount);
end
for i=1:2
V(i,:)=sqrt(R(i,i))*randn(1,maxcount);
end
%%%%%%%%%%%%%%% 生成数据 %%%%%%%%%%%%%%%%%%%%%%%%%%%%
X0=[0.1; 0.1; 1*pi/180;1*pi/180;1*pi/180; 0.0001*9.80; 0.0001*9.80; 0.3*pi/180/3600; 0.3*pi/180/3600; 0.3*pi/180/3600];
%常值漂移 0.02度每小时 弧度/秒 加速度计零偏 100ug
%%%%%%%%%%%%%%%%%%%%%%%%
x0=zeros(10,maxcount);
x0(:,1)=F1*X0+W(:,1);
yv(:,1)=C1*X0+V(:,1);
%%%%%%%%%%%%%%%%% Kalman Filter %%%%%%%%%%%%%%%%%%%%
x1=zeros(10,maxcount);%初始状态为零
x=zeros(10,1);
x1(:,1)=x;P=PP;p(1)=P(5,5);
%%%%%%%%%%%%%%%%%%%
for i=2:maxcount
%*********************************************************************
cbs = [cos(w1(i)*i*Ts) sin(w1(i)*i*Ts) 0;
-sin(w1(i)*i*Ts) cos(w1(i)*i*Ts) 0;
0 0 1];
csb=cbs';
%temp(:,i)=cbs'*[0.1*pi/180/3600;0.1*pi/180/3600;0.1*pi/180/3600];%可以从中看到经过调制的陀螺漂移
A=[0 2*WD 0 g 0 csb(1,1) csb(1,2) 0 0 0;
-2*WD 0 -g 0 0 csb(2,1) csb(2,2) 0 0 0;
0 0 0 WD 0 0 0 csb(1,1) csb(1,2) csb(1,3);
0 0 -WD 0 WN 0 0 csb(2,1) csb(2,2) csb(2,3);
0 0 0 -WN 0 0 0 csb(3,1) csb(3,2) csb(3,3);
0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0];
sys=ss(A,B,H,0);
[F1,G1,C1,Dd]=ssdata(sysd);
%%%%%%%%%%%%%%%%%%%生成真实数据%%%%%%%%%%%%%%%%%%%%%%%%%%%
x0(:,i)=F1*x0(:,i-1)+W(:,i);
yv(:,i)=C1*x0(:,i)+V(:,i);
%*********************************************************************
cbs = [cos(w*i*Ts) sin(w*i*Ts) 0;
-sin(w*i*Ts) cos(w*i*Ts) 0;
0 0 1];
csb=cbs';
%temp(:,i)=cbs'*[0.1*pi/180/3600;0.1*pi/180/3600;0.1*pi/180/3600];%可以从中看到经过调制的陀螺漂移
A=[0 2*WD 0 g 0 csb(1,1) csb(1,2) 0 0 0;
-2*WD 0 -g 0 0 csb(2,1) csb(2,2) 0 0 0;
0 0 0 WD 0 0 0 csb(1,1) csb(1,2) csb(1,3);
0 0 -WD 0 WN 0 0 csb(2,1) csb(2,2) csb(2,3);
0 0 0 -WN 0 0 0 csb(3,1) csb(3,2) csb(3,3);
0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0 0];
sys=ss(A,B,H,0);
sysd=c2d(sys,Ts);
[F,G,C,Dd]=ssdata(sysd);
% kalman滤波就用这个 模型来进行 并非实际的模型
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
Qk=(Q+F*Q*F')*Ts/2;
P = (F*P*F'+Qk); % P[n+1|n]
K = P*C'*inv(C*P*C'+R);
x = F*x; % x[n+1|n]
x = x + K*(yv(:,i)-C*x); % x[n|n]
P = (eye(10)-K*C)*P;
p(i)=P(5,5);
x1(:,i)=x;%输出为估计值
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
%%%%%%%%%%%%%%%%% 显示结果 %%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%输出结果%%%%%%%%%%%%%%%%%%%%%
t=[1:maxcount]';
figure(1),
plot(sqrt(p)*180/pi*60);xlabel('航向角估计方差 分');grid; %0.01*pi/WN/180 稳态误差
%figure(2),%估计误差 东向陀螺漂移 方位陀螺漂移不可观测
%subplot(521), plot(t,x1(1,:)-x0(1,:));ylabel('north velocity m/s');grid;
%subplot(522), plot(t,x1(2,:)-x0(2,:));ylabel('east velocity m/s');grid;
%subplot(523), plot(t,(x1(3,:)-x0(3,:))*180/pi*60);ylabel('north error ');grid;
%subplot(524), plot(t,(x1(4,:)-x0(4,:))*180/pi*60);ylabel('east error ');grid;
%subplot(525), plot(t,(x1(5,:)-x0(5,:))*180/pi*60);ylabel('yaw error ');grid;
%subplot(526), plot(t,(x1(6,:)-x0(6,:))/0.000001);ylabel('north accelerator ug');grid;
%subplot(527), plot(t,(x1(7,:)-x0(7,:))/0.000001);ylabel('east accelerator ug');grid;
%subplot(528), plot(t,(x1(8,:)-x0(8,:))*180/pi*3600);ylabel('north gyro drift o/h');grid;
%subplot(529), plot(t,(x1(9,:)-x0(9,:))*180/pi*3600);ylabel('east gyro drift o/h');grid;
%subplot(5,2,10), plot(t,(x1(10,:)-x0(10,:))*180/pi*3600);ylabel('yaw gyro drift o/h');xlabel('估计误差');grid;
figure(3);
subplot(521), plot(t,x1(1,:),'r--',t,x0(1,:),'-.');ylabel('north velocity');grid;
subplot(522), plot(t,x1(2,:),'r--',t,x0(2,:),'-.');ylabel('east velocity');grid;
subplot(523); plot(t,x0(3,:)*180/pi*60,'r--',t,x1(3,:)*180/pi*60);ylabel('north ESTIMATION');grid;
subplot(524); plot(t,x0(4,:)*180/pi*60,'r--',t,x1(4,:)*180/pi*60);ylabel('east ESTIMATION');grid;
subplot(525); plot(t,x0(5,:)*180/pi*60,'r--',t,x1(5,:)*180/pi*60);ylabel('yaw ESTIMATION');grid;
subplot(526), plot(t,x1(6,:)/0.000001,'r--',t,x0(6,:)/0.000001,'-.');ylabel('north accelerator ug');grid;
subplot(527), plot(t,x1(7,:)/0.000001,'r--',t,x0(7,:)/0.000001,'-.');ylabel('east accelerator ug');grid;
subplot(528), plot(t,x1(8,:)/0.000001,'r--',t,x0(8,:)/0.000001,'-.');ylabel('north gyro drift o/h');grid;
subplot(529), plot(t,x1(9,:)*180/pi*3600,'r--',t,x0(9,:)*180/pi*3600,'-.');ylabel('east gyro drift o/h');grid;
subplot(5,2,10), plot(t,x1(10,:)*180/pi*3600,'r--',t,x0(10,:)*180/pi*3600,'-.');ylabel('yaw gyro drift o/h');xlabel('估计状态与真实状态');grid;
%
figure(4);
%subplot(311), plot(t,(x1(3,:)-x0(3,:))*180/pi*60);ylabel('north error ');grid;
%subplot(312), plot(t,(x1(4,:)-x0(4,:))*180/pi*60);ylabel('east error ');grid;
%subplot(313), plot(t,(x1(5,:)-x0(5,:))*180/pi*60);ylabel('yaw error ');xlabel('估计误差');grid;
plot(t,(x1(5,:)-x0(5,:))*180/pi*60);ylabel('航向失准角误差/角分 ');xlabel('时间/秒');grid;
r1=x1(3,:)*180/pi;
r2=x1(4,:)*180/pi;
r3=x1(5,:)*180/pi;
figure(5);
plot(r1,'g');hold on;
plot(r2,'r');hold on;
plot(r3,'b');
grid;
%figure(6);plot(r3,'g');hold on;plot(r4,'r','lineWidth',2);grid;
%figure(7);
%subplot(311), plot(t,(x1(3,:)-x0(3,:))*180/pi*60,'b','lineWidth',2);ylabel('north error ');grid;hold on;plot(t,(x2(3,:))*180/pi*60,'r','lineWidth',2);
%subplot(312), plot(t,(x1(4,:)-x0(4,:))*180/pi*60,'b','lineWidth',2);ylabel('east error ');grid;hold on;plot(t,(x2(4,:))*180/pi*60,'r','lineWidth',2);
%subplot(313), plot(t,(x1(5,:)-x0(5,:))*180/pi*60,'b','lineWidth',2);ylabel('yaw error ');xlabel('估计误差');grid;hold on;plot(t,(x2(5,:))*180/pi*60,'r','lineWidth',2);
%hold off⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -