federal_rk.m

姿态定位完整的matlab算法

function federal_rk
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; 
%马尔可夫过程参数
%陀螺漂移：          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);

    tt = 0.02;               %龙格-库塔步长
    Gt = w/sqrt(tt/2);       %在数值解法中,系统噪声离散化
    fid = fopen('e:/ygm/vehicle/trace4.bin','r');   %打开轨迹数据文件
    fout = fopen('e:/ygm/vehicle/federal_rk1.bin','wb');  
    kk = 1;
    Ft2 = zeros(31,31);  ww = zeros(31,2);
    for k = 2:2:999*100
        [data, n] = fread(fid, 21+21, 'real*8');  %data [att, vn, pos, wm, vm, wb, fb] 
        %          Cnb                 vn         pos        wb           fb           vnD  posD
        Ft0 = Ft2;  
        vnD = data(4:6); posD = data(7:9);
        Ft1 = getf(Att2Mat(data(1:3)), data(4:6), data(7:9), data(16:18), data(19:21), vnD, posD);
        data(1:21) = []; vnD = data(4:6); posD = data(7:9);
        Ft2 = getf(Att2Mat(data(1:3)), data(4:6), data(7:9), data(16:18), data(19:21), vnD, posD);
        ww0 = ww(:,2);  ww = randn(31,2);
        k1 = Ft0* x0          + Gt.*ww0;
        k2 = Ft1*(x0+tt/2*k1) + Gt.*ww(:,1);
        k3 = Ft1*(x0+tt/2*k2) + Gt.*ww(:,1);
        k4 = Ft2*(x0+    k3)  + Gt.*ww(:,2);
        x0 = x0 + tt/6*(k1+2*k2+2*k3+k4);
        if mod(k,100) == 0
            Ht = geth(vnD);         zk = Ht*x0 + v.*randn(12,1); 
            % kf1
            Ft=Ft2(1:25,:); Ft(:,26:31)=[]; Zk1=zk(1:6); Ht1=Ht(1:6,:); Ht1(:,26:31)=[];
            [Xk1, Pk1] = kfilter(Ft, Xk1, Qt1, Ht1, Zk1, Rk1, Pk1, TKF, 25);
            % kf2
            Ft=Ft2; Ft(22:25,:)=[]; Ft(:,22:25)=[]; Zk2=zk(7:12); Ht2=Ht(7:12,:); Ht2(:,22:25)=[];
            [Xk2, Pk2] = kfilter(Ft, Xk2, Qt2, Ht2, Zk2, Rk2, Pk2, TKF, 27);
            
            fwrite(fout, [x0;zk;Xk1;Xk2], 'real*8');
            kk = kk+1;
            if mod(k,1000) == 0
              step=k/1000,    %进度、时间显示
            end
        end 
    end   
fclose(fid);
fclose(fout);