📄 minimax_modulated.m
字号:
%************************** psi角方程 **********
% minimax filter
%经过调制的 静止基座
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear;
clc;
maxcount=60*10;
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));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.3度每小时 弧度/秒 加速度计零偏 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);p=zeros(1,maxcount);
x1(:,1)=x;P0=PP;p(1)=P0(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);
%%%%%%%%%%%%%%%% 实验数据1 收敛速度比较快 但是稳态误差比较大%%%%%%%%%
gama=100;%备选项，500 700
Q1=F/15;%diag([0.01,0.01,0.01,0.01,0.01,0,0,0,0,0],0);;
WW=diag([(0.00005*9.8)^2, (0.00005*9.8)^2, (0.01*pi/180/3600)^2, (0.01*pi/180/3600)^2, (0.01*pi/180/3600)^2, 0 , 0 , 0 , 0 ,0],0)/30;
%VV=[0.000821654 0; 0 0.000821654];%振幅大，误差可能会比较小
VV=[0.00821654 0; 0 0.00821654];%振幅中，
%VV=[0.0121654 0; 0 0.0121654];%振幅小
%x1=zeros(10,maxcount);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Pminimax=zeros(10,maxcount);
%Qk=(Q+F*Q*F')*Ts/2;
L=inv(eye(10)-gama*Q1*P0+C'*inv(VV)*C*P0);
K = F*P0*L*C'*inv(VV);
P0=F*P0*L*F'+WW;
P0=(P0+P0')/2;
x1(:,i) =F*x1(:,i-1); % x[n+1|n]
x1(:,i) =x1(:,i)+ K*(yv(:,i)-C*x1(:,i)); % x[n|n]
Pminimax(:,i)=P0(5,5);
%%%%%%%%%%%%% kalman filter %%%%%%%%%%%%%%%%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)=P0(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,:),'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(3);
%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;
figure(4);
r1=x1(3,:)*180/pi;
r2=x1(4,:)*180/pi;
r3=x1(5,:)*180/pi;
%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 + -