www.pudn.com > SINS-MatlabImplement.rar > test_align_kalman.m, change:2009-02-05,size:1891b


clear 
close 
glvs 
Ts = 0.1; 
att = [0; 0; 30]*glv.deg;   qnb0 = a2qnb(att);   % 姿态真值 
phi = [10; 10; 60]*glv.min;  qnb = qaddphi(qnb0, phi); % 加入姿态误差 
Lat = 34*glv.deg; 
wnie = glv.wie*[0; cos(Lat); sin(Lat)]; 
wbib = qmulv(qconj(qnb0),wnie); 
gn = [0; 0; -glv.g0]; 
fb = qmulv(qconj(qnb0),-gn); 
 
eb = [0.01; 0.01; 0.01]*glv.dph; web =  [0.001; 0.001; 0.001]*glv.dph; % 陀螺误差 
db =  [100; 100; 100]*glv.ug;  wdb =  [10; 10; 10]*glv.ug; % 加速度计误差 
% 滤波器参数,参见《捷联惯导系统静基座初始对准精度分析及仿真》 
Qt = diag([web; 0;0])^2; 
Rk = diag([wdb(1);wdb(2)])^2; 
xk = zeros(5,1); 
Pk = diag([[1;1;10]*glv.deg; [1;1]*glv.dph])^2; 
Phi = [ 0 wnie(3) -wnie(2)   0 0  
      -wnie(3) 0 0    -1 0  
      wnie(2) 0 0     0 -1  
      zeros(2,5) ]; 
Phikk_1 = eye(5)+Phi*Ts; 
Hk = [ 0 -glv.g0  0 0 0 
      glv.g0 0   0 0 0 ]; 
  
t = 300;  % 总时间长度 
len = fix(t/Ts); res=zeros(len,3); 
feedback=0.05;  % 反馈系数 
s = 1.001; % 遗忘因子 
 
for k=1:1:len 
    wbe = wbib+eb+web.*randn(3,1); 
    fbe = fb+db+wdb.*randn(3,1); 
    qnb = qmul(qnb, rv2q( (wbe-qmulv(qconj(qnb),wnie))*Ts ));  % 姿态更新 
    fn = qmulv(qnb, fbe);   
    zk = fn(1:2);  % 以水平比力为观测量 
    [xk, Pk, Kk] = kalman(Phikk_1, Qt*Ts, xk, Pk, Hk, Rk, zk); 
%     if k*Ts>50  
%         feedback=0.01; 
%     else 
%         feedback=0; 
%     end 
%     qnb = qdelphi(qnb, feedback*xk(1:3,1));  xk(1:3,1) = (1-feedback)*xk(1:3,1); % 反馈 
%     Pk = Pk*s;  % 遗忘滤波 
    res(k,:) = [xk(1:3)-qq2phi(qnb,qnb0)]';  % 滤波估计值 - 计算平台误差 
end 
time = [1:length(res)]*Ts; 
figure; 
subplot(3,1,1); plot(time,res(:,1)/glv.min); ylabel('\it\delta\phi_E\rm / arcmin'); grid on 
subplot(3,1,2); plot(time,res(:,2)/glv.min); ylabel('\it\delta\phi_N\rm / arcmin'); grid on 
subplot(3,1,3); plot(time,res(:,3)/glv.min); ylabel('\it\delta\phi_U\rm / arcmin'); grid on 
xlabel('\itt \rm / s');