stirling.m

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

function
[xest,Pest,xpred,Ppred,inovation]=STI(xest,Pest,Q,R,Z,t)
% 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)
%             - h                : interval length h^2=3
% 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
%% PURPOSE : To estimate the states of the following nonlinear, 
%           nonstationary state space model:
%           x(t+1) = 0.5x(t) + [25x(t)]/[(1+x(t))^(2)] 
%                    + 8cos(1.2t) + process noise
%           y(t) =  x(t)^(2) / 20  + measurement noise  
%
% 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.
%x = zeros(2,T);
%processNoise = zeros(T,1);
%measureNoise = zeros(T,1);
%R = 1.0;                   % stirling's measurement noise variance. 
%Q = 10.0;                 % stirling's process noise variance.
%x0=0.1;
%y0=10;
%x(:,1)=x0;
%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);
z=zeros(1,T);
%P0 =2;       % stirling's initial variance 
h=sqrt(3);                   % interval length
%  RMSE=zeros(2,T);             % rmse
% 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)) + measureNoise(t);   % Gaussian likelihood.
%end 
% ==================
%    开始滤波
%for m=1:Num    % Num monte-carlo time

% Initial state.
% 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=sqrt(Q);       % Q=0.01^2
Sw=sqrt(R);        % R=0.04
nx=size(xest);
nv=size(Q);
nw=size(R);
%   
Sx_pre=zeros(nx,T);
Sx_est=zeros(nx,T);
Sx_est(:,1)=sqrt(P0); 
Sx_pre(:,1)=sqrt(P0);
%Sx_pre1=zeros(nx,2*(nx+nv),T);
%Sx_est1=zeros(nx,2*(nx+nw),T);
%Syy1=zeros(1,2*(nx+nw),T);
Syy=zeros(1,T);
Pxy=zeros(nx,T);
%%
%Sx_pre1=zeros(nx,2nx+2nv,T);
%Sx_est1=zeros(nx,2nx+2nw,T);
%syy1=zeros(nx,2nx+2nw,T);
%Sx_pre(:,:,1)=[0.1 0;0 0.01];
% initial the square cholesky matix
Sxx1=zeros(nx,nx,T);
Sxx2=zeros(nx,nx,T);
%%
Sxv1=zeros(nx,nv,T);
Sxv2=zeros(nx,nv,T);
%%
Syx1=zeros(1,nx,T);
Syx2=zeros(1,nx,T);
%%
Syw1=zeros(1,T);
Syw2=zeros(1,T);
%%
fxf=zeros(nx,nx,T);
fxb=zeros(nx,nx,T);
fx=zeros(nx,T);
%%
fxvf=zeros(nx,T);
fxvb=zeros(nx,T);
fxv=zeros(nx,T);
%%
gxf=zeros(1,nx,T);
gxb=zeros(1,nx,T);
gx=zeros(1,T);
%%
gywf=zeros(1,T);
gywb=zeros(1,T);
gyw=zeros(1,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
for t=2:T
Sx_est(:,1)=sqrt(P0); 
Sx_pre(:,1)=sqrt(P0);
% computing the four sguare cholesky factorizations ;
%%
fxf(1,t-1)=f(x_est(:,t-1)+h*Sx_est(:,t-1));
fxb(1,t-1)=f(x_est(:,t-1)-h*Sx_est(:,t-1));
fx(:,t-1)=f(x_est(:,t-1));
Sxx1(:,t-1)=(1/(2*h))*(fxf(:,t-1)-fxb(:,t-1));
Sxx2(:,t-1)=sqrt(h^2-1)/(2*(h^2))*(fxf(:,t-1)+fxb(:,t-1)-2*fx(:,t-1));

fxvf(:,t-1)=f(x_est(:,t-1),h*Sv);
fxvb(:,t-1)=f(x_est(:,t-1),-h*Sv);
fxv(:,t-1)=f(x_est(:,t-1));
Sxv1(:,t-1)=(fxvf(:,t-1)-fxvb(:,t-1))/(2*h);
Sxv2(:,t-1)=sqrt(h^2-1)/(2*h^2)*(fxvf(:,t-1)+fxvb(:,t-1)-2*fxv(:,t-1));        


%%    Sx_pre(k)=[Sxx1(k-1) Sxv1(k-1) Sxx2(k-1) Sxv2(k-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/(2*(h^2))*(f(x_est(:,t-1)+h*Sx_est(:,1,t-1))+f(x_est(:,t-1))-h*Sx_est(:,1,t-1)))...
+1/(2*h^2)*(f(x_est(:,t-1)+h*Sx_est(:,2,t-1))+f(x_est(:,t-1)-h*Sx_est(:,2,t-1)))...
+1/(2*h^2)*(f(x_est(:,t-1),h*Sv)+f(x_est(:,t-1),-h*Sv));                          
%%
Sx_pre1(:,:,t)=[Sxx1(:,:,t-1) Sxv1(:,t-1) Sxx2(:,:,t-1) Sxv2(:,t-1)];
Sx_pre(:,:,t)=(MGSQR((Sx_pre1(:,:,t))'))';               %     Orthogonalization 
%  posteriori update
%  computing the four sguare cholesky factorizations ;
for j=1:nx
gxf(1,j,t)=g(x_pre(1,t)+h*Sx_pre(1,j,t));
gxb(1,j,t)=g(x_pre(1,t)-h*Sx_pre(1,j,t));
gx(1,t)=g(x_pre(1,t));
Syx1(1,j,t)=(gxf(1,j,t)-gxb(1,j,t))/(2*h);
Syx2(1,j,t)=sqrt(h^2-1)/(2*h^2)*(gxf(1,j,t)+gxb(1,j,t)-2*gx(1,t));
end
%%
gywf(:,t)=g(x_pre(1,t),h*Sw);
gywb(:,t)=g(x_pre(1,t),-h*Sw);
gyw(:,t)=g(x_pre(1,t));
Syw1(:,t)=(gywf(1,t)-gywb(1,t))/(2*h);
Syw2(:,t)=sqrt(h^2-1)/(2*h^2)*(gywf(1,t)+gywb(1,t)-2*gyw(1,t));
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(Syy(:,:,t)*(Syy(:,:,t))');
y_pre(:,t)=(h^2-nx-nw)/(h^2)*g(x_pre(1,t))...
    +1/(2*h^2)*(g(x_pre(1,t)+h*Sx_pre(1,1,t))+g(x_pre(1,t)-h*Sx_pre(1,1,t)))...
    +1/(2*h^2)*(g(x_pre(1,t)+h*Sx_pre(1,2,t))+g(x_pre(1,t)-h*Sx_pre(1,2,t)))...
    +1/(2*h^2)*(g(x_pre(1,t),h*Sw)+g(x_pre(1,t),-h*Sw));
x_est(:,t)= x_pre(:,t)+Kt*(z(t)-y_pre(:,t));

% the ovariance matrix
Sx_est1(:,:,t)=[Sx_pre(:,:,t)-Kt*Syx1(:,:,t) Kt*Syw1(:,t) Kt*Syx2(:,:,t) Kt*Syw2(:,t)];
Sx_est(:,:,t)=(MGSQR((Sx_est1(:,:,t))'))';
P_est(:,:,t)=Sx_est(:,:,t)*(Sx_est(:,:,t))';
%
end              % the end of t loop 

% calculate variance of RMSE errors
for t=1:T
  RMSE(:,t)=RMSE(:,t)+(x_est(:,t)-x(:,t)).^2;             %   +(X_estimate(2,k)-x_true1(2,k))^2       
  % 计算滤波值的 RMSE
  % 计算测量值的 RMSE
end      
end
RMSE(:,t)=sqrt(RMSE(:,t)/Num);
%   输出误差曲线
t=1:T;
plot(t, RMSE(2,t));