federal.m

姿态定位完整的matlab算法

function federal
global Re e g0 wie tao Rv
Re = 6378160;   e = 1/298.3;   wie = 7.2921151467e-5;   g0 = 9.7803267714;
ppm = 1.0e-6;   ug = 1.0e-6*g0;  deg = pi/180;    min = deg/60;    sec = min/60;    hur = 3600;  dph = deg/hur; 
PKG = 0.932*sec;    PKA = 1/2500*g0;           %脉冲当量
%马尔可夫过程参数
%陀螺漂移：          eb_=-1/tao(1:3)*eb+web;       
%里程仪刻度系数误差：dKD_=-1/tao(4)+wdKD;
%GPS：         速度：dvnS_=-1/tao(5:7)*dvnS+wdvnS;
%              位置：dposS_=-1/tao(8:10)*dposS+wdposS; 
tao = [3600; 3600; 3600;   10;   5; 5; 5; 10; 10; 10];
Rv = [(0.1*dph)^2; (0.1*dph)^2; (0.1*dph)^2;  0.03^2;  0.1^2; 0.1^2; 0.1^2; (20/Re)^2; (20/Re)^2; 50^2];
%系统状态初值设置
fi = [.7 ; .5; 20]*min;     dvn = [0.1; 0.1; 0.1];           dpos = [90/Re; 90/Re; 10];	
dKG = [100; 90; 80]*ppm;    eb = [0.1; 0.1; 0.1]*dph;  
dKA = [100; 90; 80]*ppm;	db = [100; 90; 80]*ug;
dposD = [90/Re; 90/Re; 10];	dKD = 0.03;        
dvnS = [0.1; 0.1; 0.1];     dposS = [20/Re; 20/Re; 50];	
x0 = [fi; dvn; dpos; dKG; eb; dKA; db; dposD; dKD; dvnS; dposS];
%系统方程白噪声均方差
wfi = [.01; 0.01; 0.01]*min; wdvn = [0.001; 0.001; 0.001];    wdpos = [1/Re; 1/Re; 1];
wdKG = [0; 0; 0];            web = sqrt( 2*Rv(1:3).*(1./tao(1:3)) );
wdKA = [0; 0; 0];            wdb = [0; 0; 0];
wdposD = [1/Re; 1/Re; 1];    wdKD = sqrt(2*Rv(4)*1/tao(4));
wdvnS = sqrt( 2*Rv(5:7).*(1./tao(5:7)) );    wdposS = sqrt( 2*Rv(8:10).*(1./tao(8:10)) );
w = [wfi; wdvn; wdpos; wdKG; web; wdKA; wdb; wdposD; wdKD; wdvnS; wdposS];
%观测方程白噪声均方差
v = [0.01; 0.01; 0.01; 9/Re; 9/Re; 1;     0.01; 0.01; 0.01; 9/Re; 9/Re; 1];
%卡尔曼滤波时间
TKF = 1;     
% KF1:  X1_=Ft1*X1+w1        E(w1)=0, Cov(w1)=Qt1
%       Z1 =Ht1*X1+v1        E(v1)=0, Cov(v1)=Rk1
w1=w(1:25); v1=v(1:6); x10=x0(1:25);
Qt1=diag(w1.^2);     Rk1=diag(v1.^2);    Pk1=diag(x10.^2);    Xk1=zeros(25,1);
% KF2:  X2_=Ft2*X2+w2        E(w2)=0, Cov(w2)=Qt2
%       Z2 =Ht2*X2+v2        E(v2)=0, Cov(v2)=Rk2
w2=[w(1:21);w(26:31)]; v2=v(7:12); x20=[x0(1:21);x0(26:31)];
Qt2=diag(w2.^2);     Rk2=diag(v2.^2);    Pk2=diag(x20.^2);    Xk2=zeros(27,1);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 初始设置
    dG = [0, 0, 0; 0, 0, 0; 0, 0, 0]*ppm;    dA = [0, 0, 0; 0, 0, 0; 0, 0, 0]*ppm;
    KG = diag(ones(3,1)+dKG)+dG; KA = diag(ones(3,1)+dKA)+dA;
    dvnD = [0; 0; 0];
    % 捷联算法设置
    Tm  = 0.02;    
    fid = fopen('e:/ygm/vehicle/trace4.bin','r');   %打开轨迹数据文件,读数据进行初始化
    [data, n] = fread(fid, 21, 'real*8');  %data [att, vn, pos, wm, vm, wb, fb] 
    qnb = Att2Quat(data(1:3));    vnm = data(4:6);    posm = data(7:9);
    qnb = QuatMul(qnb, Rv2Quat(fi));  vnm = vnm + dvn;  posm = posm + dpos;  %初始误差
    % 里程仪算法设置
    TD = 0.1;
    vnD = data(4:6); posD = data(7:9);
    vnD = vnD + dvnD; posD = posD + dposD;    %初始误差
    % GPS设置
    TS = 1.0;
    % 马尔可夫过程离散参数
    e_tao = exp([-1./tao(1:3)*Tm; -1./tao(4)*TD; -1./tao(5:10)*TS]);
    sqw = sqrt( Rv.*(ones(10,1)-exp(-2*[1./tao(1:3)*Tm; 1./tao(4)*TD; 1./tao(5:10)*TS])) );
    
    fout = fopen('e:/ygm/vehicle/kf3.bin','wb');
    wvm2 = data(10:15);
    for k=2:2:990*100  
        %读数据,获得角增量比力增量
        [data, n] = fread(fid, 21+21, 'real*8');  
        wvm0 = wvm2; wvm1 = data(10:15); wvm2 = data((21+10):(21+15));
        dwvm1 = wvm1-wvm0; dwvm2 = wvm2-wvm1;
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 1惯导更新
        ns = randn(3,2);
        eb = e_tao(1:3).* eb  + sqw(1:3).*ns(:,1);
        dwvm1 = [KG*dwvm1(1:3); KA*dwvm1(4:6)] + [eb; db]*Tm/2;
        eb = e_tao(1:3).* eb  + sqw(1:3).*ns(:,2);
        dwvm2 = [KG*dwvm1(1:3); KA*dwvm1(4:6)] + [eb; db]*Tm/2;        %刻度系数、漂移
        [qnb, vnm, posm] = sins(qnb, vnm, posm, dwvm1, dwvm2, Tm);
        if mod(k, 10)==0
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2里程仪更新       
            dKD = e_tao(4)*dKD + sqw(4)*randn(1,1);
            vD = (1+dKD) * sqrt(sum(data(4:6).^2));
            Cnb = Quat2Mat(qnb);
            vnD = Cnb * [0; vD; 0];
            sl = sin(posD(1)); cl = cos(posD(1)); 
            RM = Re*(1-2*e+3*e*sl^2); RN = Re*(1+e*sl^2); RMhD = RM + posD(3); RNhD = RN + posD(3);
            posD = posD + TD*[vnD(2)/RMhD; vnD(1)/(RNhD*cl); vnD(3)];
            if mod(k,100)==0      
                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 3GPS更新  
                dvpS = e_tao(5:10).*[dvnS;dposS] + sqw(5:10).*randn(6,1);
                vnS = data(4:6) + dvpS(1:3);      posS = data(7:9) + dvpS(4:6);
                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4卡尔曼滤波
                Ft = getf(Cnb, vnm, posm, data(16:18)+eb, data(19:21)+db, vnD, posD);
                Ht = geth(vnD);
                % kf1
                Ft1=Ft(1:25,:); Ft1(:,26:31)=[]; Ht1=Ht(1:6,:); Ht1(:,26:31)=[];
                Zk1=[vnm-vnD; posm-posD];
                [Xk1, Pk1] = kfilter(Ft1, Xk1, Qt1, Ht1, Zk1, Rk1, Pk1, TKF, 25);
                % kf2
                Ft2=Ft; Ft2(22:25,:)=[]; Ft2(:,22:25)=[]; Ht2=Ht(7:12,:); Ht2(:,22:25)=[];
                Zk2=[vnm-vnS; posm-posS];
                [Xk2, Pk2] = kfilter(Ft2, Xk2, Qt2, Ht2, Zk2, Rk2, Pk2, TKF, 27);
                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 5联邦滤波
                %P11_1=Pk1(1:21,1:21)^-1;
                %P22_1=Pk2(1:21,1:21)^-1;
                %Pg=(P11_1+P22_1)^-1;
                %xg=Pg*(P11_1*Xk1(1:21,1)+P22_1*Xk2(1:21,1));
                %%%%%  fi    dvn   dpos   dKG   eb    dKA   db  22  dposD dKD  26  dvnS  dposS
                E = Quat2Mat(qnb)*Att2Mat(data(1:3))'; fi = -[-E(2,3);E(1,3);-E(1,2)];
                Xk = [fi; vnm-data(4:6); posm-data(7:9); dKG; eb; dKA; db; posD-data(7:9); dKD; dvpS];
                fwrite(fout, [Xk; Zk1; Zk2; Xk1; Xk2], 'real*8');
                if mod(k,1000) == 0
                    step=k/1000,    %进度、时间显示
                end
            end %end 1000
        end  %end 100
    end   %end for
fclose(fid);
fclose(fout);