ukfdemo.m

自己写的UKF滤波程序

%   
%   Unscented Kalman Filter (UKF)
%

clear all;
clc;
echo off;

% INITIALISATION AND PARAMETERS:
% ==============================

sigma =  1e-5;              % Variance of the Gaussian measurement noise.
g1 = 3;                     % Paramater of Gamma transition prior.
g2 = 2;                     % Parameter of Gamman transition prior.
                            % Thus mean = 3/2 and var = 3/4.
T = 120;                     % Number of time steps.
R = 1e-5;                   % EKF's measurement noise variance. 
Q = 3/4;                    % EKF's process noise variance.
P0 = 3/4;                   % EKF's initial variance of the states.

kappa = 2;                  % UKF : sigma point selection scaling parameter (best to leave this = 0)


% MAIN LOOP


  rand('state',sum(100*clock));   % Shuffle the pack!
  randn('state',sum(100*clock));   % Shuffle the pack!
  

% GENERATE THE DATA:
% ==================

x = zeros(T,1);
y = zeros(T,1);
processNoise = zeros(T,1);
measureNoise = zeros(T,1);
x(1) = 1;                         % Initial state.
for t=2:T
  processNoise(t) = gengamma(g1,g2);  
  measureNoise(t) = sqrt(sigma)*randn(1,1);    
  x(t) = feval('ffun',x(t-1),t) +processNoise(t);     % Gamma transition prior.  
  y(t) = feval('hfun',x(t),t) + measureNoise(t);      % Gaussian likelihood.
end;  

% INITIALISATION:
% ==============
mu_ukf = ones(T,1);       % UKF estimate of the mean of the states.
P_ukf = P0*ones(T,1);     % UKF estimate of the variance of the states.

disp(' ');

X=UKF(y,1,diag(P0),'ffun','hfun',Q,R,1);

figure
plot(1:T,x,'r',1:T,X,'g');
grid on;

figure
eX=X-x;
plot(eX);