EOT_STGP_MATLAB_CODE.zip_GPCPF_shutol7_卷积滤波_目标跟踪_跟踪

% This is a demo for evaluating the performance of STGP model based % EOT on simulated data. % Author : Waqas Aftab <waftab1@sheffield.ac.uk> % Date : 29/12/2018 % clear matlab memory clear all; close all; clc; % Simulated scenario. Five different scenarios are simulated. % 'S1' = 3 vertices, 'S2' = 4-vertices , 'S3' = 12-gon , % 'S4' = axis symmetric, 'S5' = irregular scenario = 'S2'; %%%%% STGP Model parameters [F_E_delta,Q_E_delta,dieker,ddieker,ddiiker,dC_x,... dCpp_x,dcos_x,dsin_x,dC_y,dCpp_y,dcos_y,dsin_y,dS_x,dS_y,deS_x,... deS_y,ker] = sym_expressions; % MC parameters MCruns =100; % Number of MC runs % Time parameters K = 250; % Total time steps Ts = 1/30; % Sampling time T_t = K*Ts; % Total time B = 24; % Number of keypoints % Memory Allocation Ground Truth / STGP-EKF / GP-EKF true_state = zeros(MCruns,K,4+3*B); % Ground truth state true_cog = zeros(MCruns,K,4); % Ground truth cog x_f = zeros(MCruns,4+3*B,K); % filtered estimates P_f = zeros(MCruns,4+3*B,4+3*B,K); % filtered covariance x_s = x_f; % smoother state P_s = P_f; % smoother covariance x_p = zeros(MCruns,4+3*B,K); % Predicted state xf_output = zeros(MCruns,4+3*360,K); % filtered output stgp_f_cog = zeros(MCruns,4,K); % stgp filtered cog xs_output = zeros(MCruns,4+3*360,K); % smoothed output stgp_s_cog = zeros(MCruns,4,K); % stgp smoothed cog P_p = zeros(MCruns,4+3*B,4+3*B,K); % Predicted state covariance x_gp = zeros(MCruns,4+360,K);% GP-EKF state vector gp_cog = zeros(MCruns,4,K); % GP-EKF cog % Performance Validation parameters STGP_F_P = zeros(MCruns,K);% STGP filter precision STGP_F_R = zeros(MCruns,K);% STGP filter recall STGP_S_P = zeros(MCruns,K);% STGP smoother precision STGP_S_R = zeros(MCruns,K);% STGP smoother recall GP_P = zeros(MCruns,K);% GP precision GP_R = zeros(MCruns,K);% GP recall STGP_time = zeros(MCruns,K);% STGP computation time GPEKF_time = zeros(MCruns,K);% GP computation time % Figure / Video Properties fontSize = 10; % font sixe Total_time = min(25,ceil(Ts*K)); % total time of video in seconds framerate = K / Total_time;% video frame rate for mc = 1:MCruns % STGP-EKF Basis points switch scenario case 'S1' No_of_vertices = 3; % Number of vertices case 'S2' No_of_vertices = 4; % Number of vertices case 'S3' No_of_vertices = 12; % Number of vertices case 'S4' No_of_vertices = 12; % Number of vertices case 'S5' No_of_vertices = 12; % Number of vertices end theta_E = 0:2*pi/B:2*pi-(2*pi/B); % Value of theta at B keypoints theta_b = reshape(repmat(reshape(theta_E',1,[]),3,1) ... ,[],size(theta_E,1))'; % repeat each entry thrice for pos , vel and acc % GP-EKF GP parameters Nalpha = B; % No. of inducing points (equally spaced on [0,2*pi) l_gp = deg2rad(15); % GP length scale var_r = 1; % Shape variance var_f = 1; % Magnitude variance GPEKF_gamma = 0.001; % Decay constant - forgetting factor alpha = linspace(0, 2*pi, Nalpha+1); % Inducing points alpha = alpha(1:Nalpha); % GP-EKF gradients % Covariance function and it's derivative w.r.t. x1 GPEKF_k = @(x_1, x_2) k_periodic(x_1, x_2, l_gp, var_f) + var_r; dk = @(x_1, x_2) dk_periodic(x_1, x_2, l_gp, var_f); % STGP-EKF GP parameters var_f = var_f; % Variance amplitude of spatial covariance kernel p = 2; % Periodicity of the spatial covariance kernel l_phi = l_gp; % Length scale of spatial covariance kernel var_r = var_r; % Variance of mean radial extent of spatial covariance kernel theta0 = [var_f p l_phi var_r]; % Initial / Fix hyperparameters switch scenario case 'S1' sim_p = 1/3; % hyperparameter for simulated data case 'S2' sim_p = 1/4; % hyperparameter for simulated data case 'S3' sim_p = 1/12; % hyperparameter for simulated data case 'S4' sim_p = 1; % hyperparameter for simulated data case 'S5' sim_p = 2; % hyperparameter for simulated data end sim_theta0 = [var_f sim_p l_phi var_r]; % Initial / Fix hyperparameters for simulation % STGP-EKF Kinematics model (CV model) F_K = [1 Ts 0 0;0 1 0 0;0 0 1 Ts;0 0 0 1]; % Kinematic states transition q_x = 1^2; % variance of process noise in x q_y = q_x; % variance of process noise in y Q_K = [q_x*Ts^3/3 q_x*Ts^2/2 0 0;q_x*Ts^2/2 q_x*Ts 0 0;0 0 q_y*Ts^3/3 ... q_y*Ts^2/2;0 0 q_y*Ts^2/2 q_y*Ts]; % Process noise covariance % GP-EKF kinematics Sv = q_x; % Velocity spectral density GPEKF_F = gp_model_F(Ts, alpha, GPEKF_gamma); GPEKF_Q = gp_model_Q(Ts, Sv, alpha, GPEKF_gamma); % STGP-EKF Extent paramters (Matern + Periodic Spatio-temporal model) nu = 5/2; % Matern covariance paramter nu p_e = nu-0.5; l_t = 2; % Length scale of matern covariance lambda_t = sqrt(2*nu)/l_t; % Lambda for (temporal) matern covariance var_t = 1; % Variance in magnitude of temporal covariance q_t = (2*var_t*sqrt(pi)*lambda_t^(2*p_e+1)*gamma(p_e+1))/(gamma(p_e+0.5));... % Temporal process noise variance for extent states % STGP-EKF Extent model matrices F_i_E = double(subs(F_E_delta)); F_E = kron(eye(B),F_i_E);% Extent states transition Q_delta = double(subs(Q_E_delta)); C_ee = GetGPCov(ker,theta_b,theta_b,theta0,1,0); invC_ee = inv(C_ee); Q_E = q_t*C_ee*kron(eye(B),Q_delta);% Extent model process noise % STGP-EKF Combined Model F = [F_K zeros(4,3*B);zeros(3*B,4) F_E]; % State transition matrix Q = [Q_K zeros(4,3*B);zeros(3*B,4) Q_E]; % Process noise covariance % Measurement paramters var_rho = .25^2; % variance in range error var_th = deg2rad(.25)^2; % variance in angle error lambda_m = 20; % Mean number of measurements (Poisson Model) % STGP-EKF Smoother paramteres l_s = ceil(l_t/Ts); % smoother lag % Simulated Data Parameters % %% Model Mismatch paramters th_v = deg2rad(0:360/No_of_vertices:360-(360/No_of_vertices)); theta_v = reshape(repmat(reshape(th_v',1,[]),3,1) ... ,[],size(th_v,1))'; % repeat each entry thrice for pos , vel and acc sim_C_ee = GetGPCov(ker,theta_v,theta_v,sim_theta0,1,0); % Singer Model var_m = 12; s_alpha = 1; at = s_alpha*Ts; sim_F_i_E =[1 Ts (s_alpha*Ts-1+exp(-at))/s_alpha^2;0 1 (1 - exp(-at))/s_alpha; 0 0 exp(-at)]; q11 = (1-exp(-2*at)+2*at+(2*at^3/3)-2*(at)^2-4*at*exp(-at))/(2*s_alpha^5); q12 = (exp(-2*at)+1-2*exp(-at)+2*at*exp(-at)-2*at+at^2)/(2*s_alpha^4); q13 = (1-exp(-2*at)-2*at*exp(-at))/(2*s_alpha^3); q21=q12; q22 = (4*exp(-at)-3-exp(-2*at)+2*at)/(2*s_alpha^3); q23 = (exp(-2*at)+1-2*exp(-at))/(2*s_alpha^2); q31=q13; q32=q23; q33 = (1-exp(-2*at))/(2*s_alpha); sim_Q_i_E = 2*s_alpha*var_m*[q11 q12 q13;q21 q22 q23;q31 q32 q33]; sim_F_E = kron(eye(No_of_vertices),sim_F_i_E); sim_Q_E = sim_C_ee*kron(eye(No_of_vertices),sim_Q_i_E); r0 = 25; % radius at initiation r_mink = 1; % Minimum radius throughtout sim x0 = [-45;10;-45;10]; % Initial kinematics state % STGP-EKF output parameters B_output = 360; % Output estimate number of basis theta_o = deg2rad(0:360/B_output:360-(360/B_output)); N_o = size(theta_o,2); theta_o = reshape(repmat(reshape(theta_o',1,[]),3,1) ... ,[],size(theta_o,1))'; C_oe = GetGPCov(ker,theta_o,theta_b,theta0,1,0); % Memory allocation GP-EKF N=K; Nx = 4+Nalpha; oalpha = B_output; GPEKF_theta = 0:360/oalpha:360-(360/oalpha); GPEKF_N_o = size(GPEKF_theta,2); Kaa = calculate_gp_covariance(alpha,alpha, GPEKF_k); Kta = calculate_gp_covariance(deg2rad(GPEKF_theta), alpha, GPEKF_k); mc r_0 = mvnrnd(kron(r0*ones(No_of_vertices,1),[1;0;0]),sim_Q_E)'; x_0 = [x0;