estmtda.m

自己编写的鲁棒滤波算法的应用

function [Dtz] = EstmtDa(DeA,StJ,VtJ,ts)
%***************************************************
% 利用姿态角估算方位角
% 详见惯性导航初始对准 万德钧 东南大学出版社 P144
%****************************************************
PheJ= StJ(2,:);             % 地球纬度
Ve  = VtJ(1,:);             % 机体东向速度
Re  = 6378137;              % 地球半径
wie = 0.000072921;          % 地球转动角速度

LamdaD = Ve.*sec(PheJ)/Re;   % 纬度变化率
Wu =(LamdaD+wie).*cos(PheJ); % 失准角变化天向分量
%****************************************************
% 对DetaE采用平滑化差分滤波器
%               1       _L_    x(n+N+k) - x(n+N-k)
% y(n) =  ------------- \     ----------------------
%           2N(2L+1)    /__             2
%                      k = -L
% 详见 数字信号处理 胡广书 清华大学出版社 P280
% ****************************************************
N = 60;  L=60;
x  = DeA(1,:);
[m,n]= size(x);
x_N= [zeros(1,N) x(1:n-2*N) zeros(1,N)];
xN = [zeros(1,N) x(2*N+1:n) zeros(1,N)];

Y   = (xN-x_N);
Y_2 = [zeros(1,L) zeros(1,L)   Y(1:n-3*L) zeros(1,L)];
Y_1 = [zeros(1,L)       Y(1:n-2*L)        zeros(1,L)];
Y1  = [zeros(1,L)       Y(2*L+1:n)        zeros(1,L)];
Y2  = [zeros(1,L) Y(3*L+1:n)   zeros(1,L) zeros(1,L)];
y   = (Y_2 + Y_1 + Y + Y1 + Y2)/(10*N);

y = Y/2/N;
EeD = y/ts;
Dtz= tan(PheJ) .* DeA(2,:) - EeD./Wu;