newalign.m

组合导航的程序

% Russian Alignment (经典调平)  
 
Rad_D = 0.01745329;  %Rad_Degree 
g = 9.807;
R = 6378393;
wie = 7.2921158e-5;  %弧度/秒
L = 45.7796*Rad_D;
phiz = 40*Rad_D;

phix = 1*Rad_D;
phiy = 2*Rad_D;

dvx = 0.0;
dvy = 0.0;

%K_coarse  14.0
%Kb_coarse 10.0
%K_fine     6.0
%Kb_fine    3.0

k = 2*0.707*(1.414/5);
kb = (1.414/5)*(1.414/5)/g;

alpha(1) = phix;%初始值
beta(1) = phiy;
delvx(1) = dvx;
delvy(1) = dvy;
T = 1;         %仿真步长(秒)

for i =1:600/T    
   
f_dvx = -phiy*g - k*dvx;
f_dvy = phix*g - k*dvy;
dvx = dvx + f_dvx*T;  %等价于离散化
dvy = dvy + f_dvy*T;
f_phix = -wie*cos(L)*sin(phiz)-kb*dvy;
f_phiy = -wie*cos(L)*cos(phiz)+wie*cos(L)+kb*dvx;

phix = phix + f_phix*T;
phiy = phiy + f_phiy*T;

alpha(i+1) = phix;
beta(i+1) = phiy;

delvx(i+1) = dvx;
delvy(i+1) = dvy;

end

i=1:1:600/T;
 subplot(3,2,1)
  plot(i,alpha(i)*3600/Rad_D)
  subplot(3,2,2)
  plot(i,beta(i)*3600/Rad_D)
  subplot(3,2,3)
  plot(i,delvx(i),'r')
  subplot(3,2,4)
  plot(i,delvy(i),'r')