t_ukf_kim.m

INS demo processing file

function [est_x , PA, PHI, Inno] = t_ukf_kim(F, est_x, PA, Qvec, R, Cbn, llh, svxyzmat, measvec, cl_bias, deltat)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Tightly coupled INS/GPS Unscented Kalman Filtering
%
% 坷瞒: 困摹(3), 加档(3), 磊技(3), 啊加档 bias(3), 磊捞肺 bias(3), 矫拌坷瞒(2)
%
% INPUT
%    F       : process matrix
%    est_x   : estimated error state at previous measurement update
%    PA      : a posteriori error covariance matrix at previous measurement update
%    Q       : process noise
%    Qvec    : 啊加档拌客 磊捞肺狼 process noise
%              磊技函拳啊 积辨版快 亲过谅钎拌肺 函券秦拎具 窃
%    R       : measurement noise
%    Cbn     : Direction cosine matrix
%    llh     : INS LLH Data for measurement..
%    svxyzmat: Satellite Position Matrix
%    measvec : Pseudo Range Data
%    cl_bias : GPS 荐脚扁 Clock Bias
%    deltat  : kalman filter update rate
%
% OUTPUT
%    est_x : estimated error state at current measurement update
%    PA : a posteriori error covariance matrix
%
%  Note.
%     state绰 17俺, 螟沥摹绰 GPS psuedorange and pseudorange rate 
%     05.8.22 : 漠父鞘磐狼 矫胶袍 葛胆父 荤侩窍咯 螟沥摹 葛胆篮 厚急屈 葛胆 弊措肺 捞侩窃..
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                        Unscented Filtering Algorithm
%    System Model      : INS Linear Model
%    Measurement Model : Nonlinear Model
%
%    Programmed by Kwangjin KIm
%    2005. 8. 22
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

I = eye(17,17);                 % identity matrix

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% process noise error covariance matrix
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A = diag(Qvec(1:3));            % Cbn*diag(Qvec(1:3))*Cbn';
G = diag(Qvec(4:6));            % Cbn*diag(Qvec(4:6))*Cbn';
Qm = zeros(17,17);
Qm(4:6,4:6) = A;
Qm(7:9,7:9) = G;
Qm(16,16) = Qvec(7);
Qm(17,17) = Qvec(8);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% state transition matrix
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
PHI = I + F*deltat + F^2*deltat^2 / 2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Time propagation : Linear Model
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
xb = PHI * est_x;
Pb = PHI*PA*PHI' +(PHI*Qm + Qm*PHI')*deltat/2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Sigma Point Calculation
% This Module is programmed by referencing Julier
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initial Value : Sigma Point Scaling Parameteter
alpha=0.25;
beta=2;
kappa=0;

% Calculation Sigma point
[xSigmaPts, wSigmaPts, nsp] = scaledSymmetricSigmaPoints(xb, Pb, alpha, beta, kappa);
Sigma_llh = llh*ones(1,nsp) - xSigmaPts(1:3,:);                 % Pure LLH + Sigma Point
Sigma_Cb  = cl_bias*ones(1,nsp) + xSigmaPts(16,:);              % Clock Bias + Sigma Point

% Calculation Pseudo Range Estimation Data & Measurement Mean
[numvis,nn] = size(svxyzmat);                                   % Number of Visual Satellite

for i = 1:numvis          
    for j = 1:nsp
        ins_xyz_pos = llh2xyz(Sigma_llh(:,j));                                % INS XYZ Position with Sigma Point
        pr_hat(i,j) = norm([svxyzmat(i,:) - ins_xyz_pos]) + Sigma_Cb(j);      % Y : Estimated Pseudo Range Data        
    end        
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 酒贰 何盒篮 Squared Measurement俊辑 荤侩茄 巴烙
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Squared Measurment 积己 困窍咯... numvis*2
%%         Inserted at.. 2005/10/21
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% for i = numvis+1:(numvis)
%     k = i - numvis;
%     for j = 1:nsp
%         ins_xyz_pos = llh2xyz(Sigma_llh(:,j));                                    INS XYZ Position with Sigma Point
%         pr_hat(i,j) = (norm([svxyzmat(k,:) - ins_xyz_pos]) + Sigma_Cb(j))^1; 2;      % Y : Estimated Pseudo Range Data        
%     end        
% end

% Calculation Measurement Mean
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Squared Measurment 积己 登菌栏骨肺
%%         Inserted at.. 2005/10/21
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i = 1:numvis;
    y_hat(i) = wSigmaPts(1:nsp) * pr_hat(i,1:nsp)';
end 

% Calculation Measure Variance : P_yy
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Squared Measurment档 绊妨 秦具 窍骨肺
%%         Inserted at.. 2005/10/21
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Error_Y =  pr_hat - y_hat'*ones(1,nsp);

sq_Er_Y = zeros(numvis,numvis);                       % Covariance Matrix Initialize : numvis by nimvis

for i = 2:nsp;                                        % nsp+1 : 0 Covariance Weighting
    sq_Er_Y = sq_Er_Y + wSigmaPts(i)*Error_Y(:,i)*Error_Y(:,i)';
end

P_yy =  sq_Er_Y + wSigmaPts(nsp+1)*Error_Y(:,1)*Error_Y(:,1)'+R;        % P_yy = Sigma(W*E*E')+R

% Calculation Cross Covariance : P_xy
Error_X =  xSigmaPts;
sq_Er_XY = zeros(17,numvis);
for i = 2:nsp;                                        % nsp+1 : 0 Covariance Weighting
    sq_Er_XY = sq_Er_XY + wSigmaPts(i)*Error_X(:,i)*Error_Y(:,i)';
end
P_xy =  sq_Er_XY + wSigmaPts(nsp+1)*Error_X(:,1)*Error_Y(:,1)';        % P_yy = Sigma(W*E_X*E_Y')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% measurement update
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
K = P_xy * pinv(P_yy);                                  % Kalman Gain : K = Pxy * (Pyy)^-1
PA = Pb - K * P_yy * K';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Squared Measurment 积己 登菌栏骨肺
%%  Measvec档 犬厘凳..... 2005/10/21
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
expand_meas = diag(measvec'*measvec);
measvec = [measvec]; % expand_meas'];

Inno = measvec' - y_hat';
est_x = xb + K*(Inno); %y_hat' - measvec');