UKF在GNSS/INS组合导航中的应用

UKF在组合导航中小试牛刀

% SINS/GPS intergrated navigation simulation unsing kalman filter.
% Please run 'test_SINS_trj.m' to generate 'trj10ms.mat' beforehand!!!
% See also  test_SINS_trj, test_SINS, test_SINS_GPS_186, test_SINS_GPS_193.
% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 17/06/2011
glvs
psinstypedef(153);
trj = trjfile('trj10ms.mat');
% initial settings
[nn, ts, nts] = nnts(2, trj.ts);
imuerr = imuerrset(0.03, 100, 0.001, 5);
imu = imuadderr(trj.imu, imuerr);
davp0 = avperrset([0.5;-0.5;20], 0.1, [1;1;3]);
ins = insinit(avpadderr(trj.avp0,davp0), ts);
% KF filter
rk = poserrset([1;1;3]);
kf = kfinit(ins, davp0, imuerr, rk);
kf.Pmin = [avperrset(0.01,1e-4,0.1); gabias(1e-3, [1,10])].^2;  kf.pconstrain=1;
len = length(imu); [avp, xkpk] = prealloc(fix(len/nn), 10, 2*kf.n+1);
%timebar(nn, len, '15-state SINS/GPS Simulation.'); 
ki = 1;
% for k=1:nn:len-nn+1
%     k1 = k+nn-1;  
%     wvm = imu(k:k1,1:6);  t = imu(k1,end);
%     ins = insupdate(ins, wvm);
%     kf.Phikk_1 = kffk(ins);
%     kf = kfupdate(kf);
%     if mod(t,1)==0
%         posGPS = trj.avp(k1,7:9)' + davp0(7:9).*randn(3,1);  % GPS pos simulation with some white noise
%         kf = kfupdate(kf, ins.pos-posGPS, 'M');
%         [kf, ins] = kffeedback(kf, ins, 1, 'p');
%         avp(ki,:) = [ins.avp', t];
%         xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]';  ki = ki+1;
%     end
%     timebar;
% end
% %% UKF 参数配置
L=15;     % L为状态维度
a=1;     % a控制采样点的分布状态
k=1;     % k为待选参数 没有界限，但是要保证（n+lamda）*P为半正定矩阵
b=2;     % b非负的权系数
lambda=a*a*(L+k)-L; % lambda为缩放比列参数，用于降低总的预测误差
%%%%%%%%%%%%%%%%%%%%%% sigma点的权值 %%%%%%%%%%%%%%%%%
Wm=zeros(1,2*L+1);
Wc=zeros(1,2*L+1);
Wm(1)=lambda/(L+lambda);
Wc(1)=lambda/(L+lambda)+1-a*a+b; % 第一个采样点的均值和协方差的权值
for j=2:2*L+1      % 剩下2n个sigma点的均值和协方差的权值
    Wm(j)= 1/(2*(L+lambda));  % 下标m为均值的权值
    Wc(j)= 1/(2*(L+lambda));  % 下标c为协方差的权值
end
%增加航向量测
headGPS=0.0;
kf.Hk(4,:)=zeros(1,15);kf.Hk(4,3)=1.0;
temp=zeros(4,4);temp(1:3,1:3)=kf.Rk;kf.Rk=temp;kf.Rk(4,4)=(pi*180)*(pi*180);
%% UKF 仿真
for k=1:nn:len-nn+1
    k1 = k+nn-1;  
    wvm = imu(k:k1,1:6);  t = imu(k1,end);
    ins = insupdate(ins, wvm);
    kf.Phikk_1 = kffk(ins);
    cho=(chol(kf.Pxk*(L+lambda)))';
    %cho=m_skysd(kf.Pxk*(L+lambda));
    for ks=1:L
        xgamaP1(:,ks)=kf.xk+cho(:,ks);
        xgamaP2(:,ks)=kf.xk-cho(:,ks);
    end
    Xsigma=[kf.xk xgamaP1 xgamaP2]; % 获得 2L+1 个Sigma点
    % 一步预测
    Xsigmapre=kf.Phikk_1*Xsigma;
    % 求预测后的均值
    Xpred=zeros(L,1);
    for ks=1:2*L+1
        Xpred=Xpred+Wm(ks)*Xsigmapre(:,ks);
    end
    Ppred=zeros(L,L); %协力差阵预测
    for ks=1:2*L+1
        Ppred=Ppred+Wc(ks)*(Xsigmapre(:,ks)-Xpred)*(Xsigmapre(:,ks)-Xpred)';
    end
    %Ppred=Ppred+G*Q*G';
    if mod(t,1)==0
        posGPS = trj.avp(k1,7:9)' + davp0(7:9).*randn(3,1);
        if trj.avp<-pi
            headGPS=trj.avp(k1,3)+2*pi+pi*180*randn(1);
        else
            headGPS=trj.avp(k1,3)+pi*180*randn(1);
        end
        headGPS=trj.avp(k1,3)+randn(1);
        resp=ins.pos-posGPS;
        resp(4)=ins.att(3)-headGPS;
        %根据Ut变换获得新的sigma点集
        chor=(chol((L+lambda)*Ppred))';
        for ks=1:L
            XaugsigmaP1(:,ks)=Xpred+chor(:,ks);
            XaugsigmaP2(:,ks)=Xpred-chor(:,ks);
        end
        Xaugsigma=[Xpred XaugsigmaP1 XaugsigmaP2];
        % 观测预测
        Zsigmapre=kf.Hk*Xaugsigma;
        % 计算观测预测的均值和协方差
        Zpred=zeros(4,1); %观测预测的均值
        for ks=1:2*L+1
            Zpred=Zpred+ Wm(ks)*Zsigmapre(:,ks);
        end
        Pzz=zeros(4,4);
        for ks=1:2*L+1
            Pzz=Pzz+Wc(ks)*(Zsigmapre(:,ks)-Zpred)*(Zsigmapre(:,ks)-Zpred)';
        end
        Pzz=Pzz+kf.Rk; %得到协方差Pzz
        Pxz= zeros(L,4);
        for ks=1:2*L+1
            Pxz=Pxz+Wc(ks)*(Xaugsigma(:,ks)-Xpred)*(Zsigmapre(:,ks)-Zpred)';
        end
        %计算卡尔曼增益
        K=Pxz/Pzz;
        %K=Pxz*inv(Pzz);  % Kalman
        % 状态更新和协方差更新
        kf.xk=Xpred+K*(resp-Zpred);% 状态更新
        kf.Pxk=Ppred-K*Pzz*K'; %方差更新
        % 反馈更新
        [kf, ins] = kffeedback(kf, ins, 1, 'avp');
    else
        kf.xk=Xpred;
        kf.Pxk=Ppred;
    end
    Xk(ki,:)=kf.xk';
    result_avp(ki,:) = [ins.avp', t];ki=ki+1;
end
%%
% avp(ki:end,:) = [];  xkpk(ki:end,:) = []; 
% % show results
% insplot(avp);
% avperr = avpcmpplot(trj.avp, avp);
% kfplot(xkpk, avperr, imuerr);

备注：
如有错误，勿喷，请多指点（刚刚入行的小菜鸟！）
参考文献：
感谢严恭敏老师的开源软件PSINS