stirling interpolation.asv

一个二阶中心插值滤波器（DDF2）的目标跟踪仿真实例。

% TITLE    :  STIRLING INTERPOLATION FILTER  
%
% PURPOSE  :  This function performs one complete step of the stirling interpolaqtion filter.
%
% SYNTAX   : 
% INPUTS   :  - xest             : state mean estimate at time k  
%             - Pest             : state covariance at time k
%             - Q                : process noise covariance at time k  
%             - z                : observation at k+1  
%             - R                : measurement noise covariance at k+1  
%             - ffun             : process model function  
%             - hfun             : observation model function  
%             - dt               : time step (passed to ffun/hfun)   
% OUTPUTS  :  - xest             : updated estimate of state mean at time k+1
%	          - Pest             : updated state covariance at time k+1
%             - xPred            : prediction of state mean at time k+1
%             - PPred            : prediction of state covariance at time k+1
%	          - inovation        : innovation vector
%  
%
% NOTES    :  The process model is of the form, x(k+1) = ffun[x(k),v(k),dt,u(k)]
%             where v(k) is the process noise vector. The observation model is 
%             of the form, z(k) = hfun[x(k),w(k),dt,u(k)], where w(k) is the 
%             observation noise vector.
%

clc;
clear all;

% INITIALISATION AND PARAMETERS:
% ==============================
T = 200;                    % Number of time steps.
Num=100;                    % Monte-carlo simulation time steps.
R = 0.04;                   % stirling's measurement noise variance. 
Q = 0.0001;                 % stirling's process noise variance.
RMSE=zeros(2,T);            % rmse
%    开始滤波
for m=1,Num    % Num monte-carlo time
  rand('state',sum(100*clock));   % Shuffle the pack!
  randn('state',sum(100*clock));   % Shuffle the pack!  
% Initial state.
x = zeros(2,T);
x0=[0;0.1];
y0=10;
x(:,1)=xo;
x_est0=[0.8;0.08];
x_est=zeros(2,T);
x_pre=zeros(2,T);
y_pre=zeros(1,T);
P_est=zeros(2,2,T);
P0 =[0.1 0;0 0.0001];       % stirling's initial variance 
h=sqrt(3);                 % interval length
x = zeros(2,T);
processNoise = zeros(T,1);
measureNoise = zeros(T,1);
% generate true trajectory
% t=0:ns，速度vx,vy为沿x和y轴的速度分量(m/s)
vx=0.1;
% true trajectory
for t=1:T
    x(t)=x0+[vx;0]*(t-1);
%    y(t)=y0;
end
%%%     generate measurement data:
for t=1:T
   measureNoise(t) = sqrt(R)*randn(1,1);    
   z(t) = feval('hfun',x(1,t),t) + measureNoise(t);   % Gaussian likelihood.
end 
% ==================
% introduce the following four square cholesky factorizations
% Q=Sv*Sv', R=Sw*Sw'
% P_pre=Sx_pre*Sx_pre',P_est=Sx_est*Sx_est'.
% Sx(k)=[Sxx1(k-1) Sxw1(k-1) Sxx2(k-1) Sxw2(k-1)]
% Sy(k)=[Syx1(k) Syv1(k) Syx2(k) Syv2(k)]
Sv=0.01;       % Q=0.01^2
Sw=0.2;        % R=0.04
Sx0(:,:,0)=[0.1 0;0 0.01];
Sx_pre=zeros(nx,nx,T);
Sx_est=zeros(nx,nx,T);
Sx_pre=zeros(nx,nx,T);
Sx_est=zeros(nx,nx,T);
Sx_pre1=zeros(nx,2(nx+nw),T);
Sx_est1=zeros(nx,2(nx+nw),T);
syy1=zeros(nx,2(nx+nw),T)
nx=2;
nv=1;
nw=1;
Sx_pre(:,:,1)=[0.1 0;0 0.01];
Sx_est(:,:,1)=[0.1 0;0 0.01];
% initial the square cholesky matix
for t=2,T
Sx_pre(:,:,1)=[0.1 0;0 0.01];
Sx_est(:,:,1)=[0.1 0;0 0.01];
Sxx1=zeros(nx,nx,T);
Sxx2=zeros(nx,nx,T);
Sxv1=zeros(nx,nv,T);
Sxv2=zeros(nx,nv,T);
Syx1=zeros(nx,nx,T);
Syx2=zeros(nx,nx,T);
Syw1=zeros(nx,nw,T);
Syw2=zeros(nx,nw,T);
%  [y]=fi(x1,x2)                         %%    call fi function
%  [y]=gi(x1,x2)                         %%    call gi function 
%  [y]=f(x1,x2)                          %%    call f Process model function
%  [y]=g(x1,x2)                          %%    call g mesurement function
% computing the four sguare cholesky factorizations ;
for i=1:nx
    for j=1:nx
Sxx1(i,j,t-1)=1/2h*(fi(x_est(:,t-1)+h*sx_est(:,j,t-1)-fi(x_est(:,t-1)-h*sx_est(:,j,t-1));
Sxx2(i,j,t-1)=sqrt(h^2-1)/(2h^2)*(fi(x_est(:,t-1)+h*sx_est(:,j,t-1))+fi(x_est(:,t-1)-h*sx_est(:,j,t-1)-2fi(x_est(:,t-1));
Syx1(i,j,t)=(gi(x_pre(:,t)+h*Sx_pre(:,j,t))-gi(x_pre(:,t)+h*Sx_pre(:,j,t)))/2h;
Syx2(i,j,t)=sqrt(h^2-1)/2h^2*(gi(x_pre(:,t)+h*Sx_pre(:,j,t))-gi(x_pre(:,t)+h*Sx_pre(:,j,t))-2gi(x_pre(:,t));
    end
end
for i=1:nx
    for j=1:nv
Sxv1(i,j,t-1)=(fi(x_est(:,t-1),hsv)-fi(x_est(:,t-1),-hsv))/2h;
Sxv2(i,j,t-1)=sqrt(h^2-1)/2h^2*(fi(x_est(:,t-1),hsv)+fi(x_est(:,t-1),-hsv)-2fi(x_est(;,t-1));        
    end
end
%%    Sx_pre(k)=[Sxx1(k-1) Sxw1(k-1) Sxx2(k-1) Sxw2(k-1)]
Sx_pre(:,:,t)=[Sxx1(:,:,t-1) Sxw1(:,:,t-1) Sxx2(:,:,t-1) Sxw2(:,:,t-1)]
%%%%%%%%
%    [y]=gi(x1,x2)                             call gi function 
%    [y]=g(x1,x2)                              call g mesurement function
%  one step prediction
x_pre(;,t)=(h^2-nx-nw)/(h^2)*f(x_est(:,t-1)+1/(2h^2)*(f(x_est(:,t-1))+h*Sx_est(:,:,t-1))+f(x_est(:,t-1))-h*Sx_est(:,:,t-1))...
    +f(x_est(:,t-1))+h*Sx_est(:,2))+f(x_est(:,t-1))-h*Sx_est(:,:,t))+1/2h^2*(f(x_est(:,t-1),h*Sv)+f(x_est(:,t-1),-h*Sv));
Sx_pre1(:,:,t)=[Sxx1(:,:,t-1) Sxw1(:,:,t-1) Sxx2(:,:,t-1) Sxw2(:,:,t-1)];
Sx_pre(:,:,t)=MGSQR(Sx_pre1(:,:,t));               %      Orthogonalization 
%  posteriori update
%  computing the four sguare cholesky factorizations ;
for i=1:nx
    for j=1:nx
Syx1(i,j,t)=(gi(x_pre(:,t)+h*Sx_pre(:,j,t))-gi(x_pre(:,t)+h*Sx_pre(:,j,t)))/2h;
Syx2(i,j,t)=sqrt(h^2-1)/(2h^2)*(gi(x_pre(:,t)+h*Sx_pre(:,j,t))-gi(x_pre(:,t)+h*Sx_pre(:,j,t))-2gi(x_pre(:,t));
    end
end
for i=1:nx
    for j=1:nw
Syw1(i,j,t)=(gi(x_pre(:,t),h*sw)-gi(x_pre(:,t),-h*sw))/2h;
Syw2(i,j,t)=sqrt(h^2-1)/(2h^2)*(gi(x_pre(:,t),h*sw)+gi(x_pre(:,t),-h*sw)-2gi(x_pre(:,t);
    end
end
Syy1(:,:,t)=[Syx1(:,:,t) Syx2(:,:,t) Syw1(:,:,t) Syw2(:,:,t)];
Syy(:,:,t)=MGSQR(Syy1(:,:,t));                      %      Orthogonalization    
% kalman gain 
Pxy(:,:,t)=Sx_pre(:,:,t)*(Syx1(:,:,t))';
Kt=Pxy(:,:,t)*inv(Sy(t)*Sy(t)');
y_pre(:,t)=(h^2-nx-nw)/(h^2)*g(x_pre(:,t))+1/(2h^2)*(g(x_pre(:,t)+h*Sx_pre(:,:,t))+g(x_pre(:,t)-h*Sx_pre(:,:,t)...
         +g(x_pre(:,t)+h*Sx_pre(:,:,t))+g(x_pre(:,t)-h*Sx_pre(:,:,t)+1/(2h^2)*(g(x_pre(:,t),h*sw)+g(x_pre(:,t)-h*sw));
x_est(:,t)= x_pre(;,t)+Kt*(z(t)-y_pre(:,t));
% the ovariance matrix
Sx_est(:,:,t)=[Sx_pre(:,:,t)-KtSyx1(:,:,t) KtSyw1(:,:,t) KtSyx2(:,:,t) KtSyw2(:,:,t)];
Sx_est(:,:,t)=MGSQR(Sx_est(:,:,t));
P_est(:,:,t)=Sx_est(:,:,t)*(Sx_est(:,:,t))';
end
% calculate variance of RMSE errors
for t=1:N
  RMSE(:,t)=RMSE(:,t)+(x_estimate(:,t)-x(1,t))^2;             %   +(X_estimate(2,k)-x_true1(2,k))^2       
  % 计算滤波值的 RMSE
  % 计算测量值的 RMSE
end      
end
RMSE=sqart(RMSE/Num);
%   输出误差曲线 
plot(t, RMSE(1,t));