ins_alignment_unsented.m

unsented kalman filter算法在惯性导航系统中的应用

%           1) Extended Kalman Filter  (EKF)
%           2) Unscented Kalman Filter (UKF)
% Copyright  (c) Gao Yanan (2004)
% Novel approach to nonlinear/non-Gaussian Bayesian state estimation
%   Bearings-only tracking example
clear all;
clc;
% INITIALISATION AND PARAMETERS:
% ==============================
T = 25;                     % Number of time steps.
alpha = 1;                  % UKF : point scaling parameter
beta  = 0;                  % UKF : scaling parameter for higher order terms of Taylor series expansion 
kappa = 2;                  % UKF : sigma point selection scaling parameter (best to leave this = 0)
%**************************************************************************************
% SETUP BUFFERS TO STORE PERFORMANCE RESULTS
% ==========================================
%**************************************************************************************
% MAIN LOOP
 rand('state',sum(100*clock));   % Shuffle the pack!
  randn('state',sum(100*clock));   % Shuffle the pack!
% GENERATE THE DATA:
% ==================
x = zeros(4,T);
y = zeros(1,T);
R = 0.005;                   % 测量噪声方差. 
Q = 0.005*[1 0 ;0 1]; % 过程噪声方差
sigma =  0.005;              % Variance of the Gaussian measurement noise.
processNoise = zeros(2,T);
processNoise(:,1) =sqrtm(Q)*[randn,randn]';
measureNoise = zeros(T,1);
x(:,1) =[0.56, -0.057, -0.67, 0.097]';                         % Initial state.
%bearing_0      = -pi+rand(1)*2*pi;
%bearing_rate_0 = 0.1*randn(1);
%range_0        = 0.1*randn(1)+1;
%range_rate_0   = 0.01*randn(1)-0.1;

%x(:,1) = [range_0*cos(bearing_0);(range_0 + range_rate_0)*cos(addangle(bearing_0,bearing_rate_0)) - range_0*cos(bearing_0);range_0*sin(bearing_0);(range_0 + range_rate_0)*sin(addangle(bearing_0,bearing_rate_0)) - range_0*sin(bearing_0)];
 y(:,1) = atan2(x(3,1),x(1,1))+measureNoise(1); 
 idx = find(abs(y(:,1)) > pi);
      temp = rem(y(:,idx),2*pi);
      y(:,idx) = temp - sign(temp).*(2*pi);
A=[1 1 0 0;
   0 1 0 0;
   0 0 1 1;
   0 0 0 1];
B=[0.5 0;
    1  0;
    0 0.5;
    0  1];
for t=2:T
  processNoise(:,t) = sqrtm(Q)*[randn,randn]';
  measureNoise(t) = sqrt(sigma)*randn(1,1);    
  x(:,t) =A*x(:,t-1) +B*processNoise(:,t);     % Gamma transition prior.  
  y(t) = atan2(x(3,t),x(1,t))+measureNoise(t);      % Gaussian likelihood.
end;  
% PLOT THE GENERATED DATA:
% ========================
%figure(1);plot(x(1,:),x(3,:));
%plot(1:T,x,'r',1:T,y,'b');
%ylabel('Data','fontsize',15);
%xlabel('Time','fontsize',15);
%legend('States (x)','Observations(y)');
%%%%%%%%%%%%%%%  PERFORM EKF and UKF ESTIMATION  %%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%  ==============================  %%%%%%%%%%%%%%%%%%%%%
% INITIALISATION:
% ==============
%mu_ekf = ones(T,1);     % EKF estimate of the mean of the states.
%P_ekf = x(:,1)*x(:,1)';   % EKF estimate of the variance of the states.
mu_ukf = zeros(4,T);
mu_ukf(:,1) =x(:,1);        % UKF estimate of the mean of the states.
%P_ukf =  [0.001^2 0 0 0;0 0.001^2 0 0;0 0 0.001^2 0;0 0 0 0.001^2];          % UKF estimate of the variance of the states.
P_ukf =  [0.56^2 0      0      0;
            0   0.057^2 0      0;
            0   0      0.67^2   0;
            0   0      0      0.097^2];          % UKF estimate of the variance of the states.
%P_ukf =((mu_ukf(:,1)*mu_ukf(:,1)') +(mu_ukf(:,1)*mu_ukf(:,1)' )')/2;
yPred = ones(T,1);      % One-step-ahead predicted values of y.
%mu_ekfPred = ones(T,1); % EKF O-s-a estimate of the mean of the states.
PPred = ones(T,1);      % EKF O-s-a estimate of the variance of the states.
for t=2:T,
   % Full Unscented Kalman Filter step
  % =================================
  %[mu_ukf(:,t),P_ukf]=ukf(mu_ukf(:,t-1),P_ukf,[],Q,'ukf_ffun',y(t),R,'ukf_hfun',t,alpha,beta,kappa);
   [mu_ukf(:,t),P_ukf]=ukf(mu_ukf(:,t-1),P_ukf,[],Q,'ukf_ffun',y(:,t),R,'ukf_hfun',t,alpha,beta,kappa);
end;   % End of t loop.
%%%%%%%%%%%%%%%%%%%%%  PLOT THE RESULTS  %%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%  ================  %%%%%%%%%%%%%%%%%%%%%
figure(1)
plot(x(1,:),x(3,:));hold on;
%figure(2)
plot(mu_ukf(1,:),mu_ukf(3,:),'r');
%p3=plot(1:T,mu_ukf,'b:','lineWidth',2);hold on;
%p1=plot(1:T,x,'k:o','lineWidth',2); hold off;
%legend([p1 p2 p3],'True x','EKF estimate','UKF estimate');xlabel('Time','fontsize',15);
title('Filter estimates (posterior means) vs. True state','fontsize',15)

figure(1);
p1=plot(x(1,:),x(3,:),'-*'); hold on;
p2=plot(x(1,1),x(3,1),'c*');
p3=plot(x(1,end),x(3,end),'m*');
p4=plot(0,0,'kx','linewidth',2); hold off;
legend([p1 p2 p3 p4],'target trajectory','position : k=0',['position : k=' num2str(T)],'observer position',0);
xlabel('x');
ylabel('y');
title('Bearings-Only Tracking : Target Trajectory');
figure(1); hold on;
p5=plot(mu_ukf(1,:),mu_ukf(3,:),'r-o');
plot(mu_ukf(1,1),mu_ukf(3,1),'c*');
plot(mu_ukf(1,end),mu_ukf(3,end),'m*');
legend([p1 p2 p3 p4 p5],'target trajectory','position : k=0',['position : k=' num2str(T)],'observer position','estimated trajectory',0);
xlabel('x');
ylabel('y');
title('Bearings-Only Tracking : Target Trajectory');
hold off;
%semilogy(1:T,P_ekf,'r--',1:T,P_ukf,'b','lineWidth',2);
%legend('EKF','UKF');
%title('Estimates of state covariance','fontsize',14);
%xlabel('time','fontsize',12);
%ylabel('var(x)','fontsize',12);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%-- CALCULATE PERFORMANCE --%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%rmsError_ekf(j)     = sqrt(inv(T)*sum((x-mu_ekf).^(2)));
%rmsError_ukf(j)     = sqrt(inv(T)*sum((x-mu_ukf).^(2)));
%*************************************************************************