test.asv

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

0% 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=10;                    % Monte-carlo simulation time steps.
x = zeros(2,T);
processNoise = zeros(T,1);
measureNoise = zeros(T,1);
R = 0.01;                   % stirling's measurement noise variance. 
Q = 0.0001;                 % stirling's process noise variance.
x0=[1;0.1];
y0=10;
x(:,1)=x0;
z=zeros(1,T);
P0 =[0.01 0;0 0.0001];       % 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)]
x_est=zeros(2,T);
x_pre=zeros(2,T);
y_pre=zeros(1,T);
P_est=zeros(2,2,T);
Sv=0.01;       % Q=0.01^2
Sw=0.2;        % R=0.04
nx=2;
nv=1;
nw=1;
Sx0=[0.1 0;0 0.01];
Sx_pre=zeros(nx,nx,T);
Sx_est=zeros(nx,nx,T);
%Sx_est(:,:,1)=[0.1 0;0 0.01]; 
%Sx_pre(:,:,1)=[0.1 0;0 0.01];
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,1,T);
Pxy=zeros(nx,1,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);
%%
K=ones(2,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
x_est(:,1)=[0.8;0.08];  
Sx_est(:,:,1)=[0.1 0;0 0.01]; 
Sx_pre(:,:,1)=[0.1 0;0 0.01];
% computing the four sguare cholesky factorizations ;
%%
for j=1:nx
    for i=1:nx
fxf(:,j,t-1)=f(x_est(:,t-1)+h*Sx_est(:,j,t-1));
fxb(:,j,t-1)=f(x_est(:,t-1)-h*Sx_est(:,j,t-1));
fx(:,t-1)=f(x_est(:,t-1));
Sxx1(i,j,t-1)=(1/(2*h))*(fxf(i,j,t-1)-fxb(i,j,t-1));
Sxx2(i,j,t-1)=(sqrt(h^2-1)/(2*(h^2)))*(fxf(i,j,t-1)+fxb(i,j,t-1)-2*fx(i,t-1));
    end
end

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))';  
K(:,t)=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)+K(:,t)*(z(t)-y_pre(:,t));

% the ovariance matrix
Sx_est1(:,:,t)=[Sx_pre(:,:,t)-K(:,t)*Syx1(:,:,t) K(:,t)*Syw1(:,t) K(:,t)*Syx2(:,:,t) K(:,t)*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(1,t));