├── Matlab ├── compute_GP_covariance.m ├── demo.m ├── filter_GPETT2D.m ├── generate_measurements_2D.m └── visualize_tracking_outputs.m └── README.md /Matlab/compute_GP_covariance.m: -------------------------------------------------------------------------------- 1 | function [ covMatrix ] = compute_GP_covariance(argArray1, argArray2, paramGP) 2 | % This function computes the GP covariance according to the specified parameters. 3 | % The type of the kernel function is the squared exponential. 4 | % Output: 5 | % covMatrix: The covariance matrix computed by the GP kernel. 6 | % Inputs: 7 | % argArray1: First argument of the covariance function. (Column vector) 8 | % argArray2: Second argument of the covariance function. (Column vector) 9 | % paramGP: Parameters of the specified GP 10 | 11 | 12 | % Author: Murat Kumru 13 | 14 | 15 | % Extract the parameters 16 | stdPrior = paramGP{2}; 17 | stdRadius = paramGP{3}; 18 | lengthScale = paramGP{4}; 19 | 20 | diffMatrix = compute_diffence_matrix(argArray1, argArray2); 21 | 22 | % Covariance periodic with 2*pi 23 | covMatrix = stdPrior^2 * exp(-2 * sin(diffMatrix/2).^2 / lengthScale^2) + stdRadius^2; 24 | 25 | end 26 | 27 | 28 | function [ diffMatrix ] = compute_diffence_matrix(argArray1, argArray2) 29 | 30 | len1 = length(argArray1); 31 | len2 = length(argArray2); 32 | 33 | argGrid1 = repmat(argArray1, [1, len2]); 34 | argGrid2 = repmat(transpose(argArray2), [len1, 1]); 35 | 36 | diffMatrix = argGrid1-argGrid2; 37 | end 38 | -------------------------------------------------------------------------------- /Matlab/demo.m: -------------------------------------------------------------------------------- 1 | % This file demonstrates an example utilization of the algorithm proposed 2 | ...in the following paper: 3 | 4 | % "Extended target tracking using Gaussian processes" (IEEE Trans. on Signal Process., 5 | ...vol. 63, no. 16, pp. 4165–4178, Apr. 2015) by N. Wahlström and E. Özkan 6 | 7 | % Dependencies: None 8 | 9 | 10 | % Author: Murat Kumru 11 | % Date: 07.10.2019 12 | 13 | 14 | clc; 15 | clear all; 16 | close all; 17 | 18 | %% Paramaters 19 | % Simulation parameters 20 | expDuration = 30; % Experiment duration (in seconds) 21 | T = 1; % Sampling time (in seconds) 22 | numMeasPerInstant = 20; % Number of point measurements acquired in an instant 23 | stdMeas = 0.1; % Standart deviation of the measurements 24 | isMeasurementPartial = 0; % Flag for partial measurement 25 | motionType = 2; % 1: Still, 2: Constant velocity, 3: Sinusoidal motion 26 | eps = 1e-6; % A small scalar 27 | % Object parameters 28 | objType = 2; % objType = 1:Circle, 2:Square, 3:Triangle 29 | switch objType 30 | case 1 31 | radius = 3; 32 | objParameters = radius; 33 | case 2 34 | edgeLength = 3; 35 | objParameters = edgeLength; 36 | case 3 37 | sideEdgeLength = 5; 38 | bottomEdgeLength = 4; 39 | objParameters = [sideEdgeLength bottomEdgeLength]; 40 | case 4 41 | objParameters = []; 42 | end 43 | paramMeas = {expDuration, T, numMeasPerInstant, stdMeas, objType, objParameters... 44 | , motionType, isMeasurementPartial}; 45 | 46 | % GP parameters 47 | numBasisAngles = 50; % The number of basis angles (on which the extent is to be maintained) 48 | meanGP = 0; % Mean of the GP model 49 | stdPriorGP = 2; % Prior standart deviation 50 | stdRadiusGP = 0.8; % Radius standart deviation 51 | lengthScaleGP = pi/8; % Length scale (smaller lengthscale implies edgier/ more spiky extents) 52 | stdMeasGP = stdMeas; % Measurement std in the GP model (it may be set different from the original value) 53 | paramGP = {meanGP, stdPriorGP, stdRadiusGP, lengthScaleGP, stdMeasGP}; 54 | 55 | % Process paramaters 56 | stdCenter = 1e-2; % Std dev of the position process noise 57 | stdPsi = 1e-4; % Std dev of the orientation angle (psi) process noise 58 | alpha = 1e-4; % Forgetting factor (to used in the process model of the extent) 59 | paramEKF = [stdCenter stdPsi alpha]; 60 | 61 | % Determine the basis angles 62 | basisAngleArray = transpose(linspace(0, 2*pi, numBasisAngles+1)); 63 | basisAngleArray(end) = []; % Trash the end point to eleminate the repetition at 2*pi 64 | 65 | 66 | %% Generate Measurements and Ground Truth 67 | [measComplete, groundTruth] = generate_measurements_2D(paramMeas); 68 | 69 | 70 | %% Determine the initial distribution 71 | % Initial means 72 | c0 = transpose(mean(measComplete(1:numMeasPerInstant, 2:3), 1)); % Compute Initial center as the average of the first set of measurements 73 | v0 = zeros(2,1); % Initial velocity 74 | psi0 = 0; % Initial orientation (yaw) angle 75 | psiDot0 = 0; % Initial yaw rate 76 | f0 = meanGP * ones(numBasisAngles, 1); % Initial extent 77 | % Initial covariances 78 | P0_center = 10 * eye(2); 79 | P0_velocity = 1 * eye(2); 80 | P0_psi = 1e-5; 81 | P0_psiDot = 1e-5; 82 | P0_extent = compute_GP_covariance(basisAngleArray, basisAngleArray, paramGP); 83 | P0 = blkdiag(P0_center, P0_psi, P0_velocity, P0_psiDot, P0_extent); 84 | 85 | % Initialize the filter estimate 86 | estState = [c0; psi0; v0; psiDot0; f0]; 87 | estStateCov = P0; 88 | 89 | 90 | %% Process Model 91 | F_tracking = kron([1 T; 0 1], eye(3)); % Constant velocity model 92 | F_extent = exp(-alpha*T) * eye(numBasisAngles); % A forgetting factor is included 93 | F = blkdiag(F_tracking, F_extent); 94 | 95 | Q_tracking = kron([T^3/3 T^2/2; T^2/2 T], diag([stdCenter^2 stdCenter^2 stdPsi^2])); 96 | Q_extent = (1-exp(-2*alpha*T)) * P0_extent; 97 | Q = blkdiag(Q_tracking, Q_extent); 98 | 99 | processModel = {F, Q}; 100 | 101 | 102 | %% Open a Figure (to illustrate tracking outputs) 103 | figure; 104 | grid on; hold on; 105 | ylim([min(measComplete(:,3))-2 max(measComplete(:,3))+2]); 106 | xlim([min(measComplete(:,2))-2 max(measComplete(:,2))+2]); 107 | ylabel('Y axis'); 108 | xlabel('X axis'); 109 | title('Extended Target Tracking using Gaussian Processes'); 110 | 111 | %% Initialize Simulation Log 112 | numInstants = ceil(expDuration/ T); 113 | simLog.Parameters = {paramMeas, paramGP, paramEKF, processModel, basisAngleArray}; 114 | simLog.GroundTruth = groundTruth; 115 | %simLog.GroundTruth = groundTruth; 116 | simLog.TrackingData(numInstants) = struct('time', [], 'measurement', []... 117 | , 'stateEstimated', [], 'stateCovariance', []); 118 | 119 | 120 | %% GPETT2D LOOP 121 | time = 0; 122 | for k = 1:numInstants 123 | 124 | % Extract current measurements 125 | curMeasArray = measComplete(abs(measComplete(:, 1)-time) 32 | 33 | 34 | % Extract matrices defining the proces model 35 | F = processModel{1}; 36 | Q = processModel{2}; 37 | % Extract the necessary parameters of the GP model 38 | lengthScaleGP = paramGP{4}; 39 | stdMeasGP = paramGP{5}; 40 | 41 | % Compute the inverse of P0_extent since it will repeatedly be used in the system dynamic model 42 | persistent inv_P0_extent; 43 | if isempty(inv_P0_extent) 44 | P0_extent = compute_GP_covariance(basisAngleArray, basisAngleArray, paramGP); % Covariance of the target extent 45 | P0_extent = P0_extent + eps*eye(size(basisAngleArray,1)); % To prevent numerical errors thrown during matrix inversion 46 | chol_P0_extent = chol(P0_extent); 47 | inv_chol_P0_extent = inv(chol_P0_extent); 48 | inv_P0_extent = inv_chol_P0_extent * inv_chol_P0_extent'; 49 | end 50 | 51 | 52 | %% Process Update 53 | predState = F * prevEstState; % Predicted state 54 | predStateCov = F * prevEstStateCov * F' + Q; % Covariance of the predicted state 55 | 56 | 57 | %% Measurement Update 58 | curNumMeas = size(measArray, 1); 59 | numStates = size(F,1); 60 | 61 | % Extract relevant information from the predicted state 62 | predPos = predState(1:2); 63 | predPsi = predState(3); 64 | predExtent = predState(7:end); 65 | 66 | % In the below loop, the following operations are performed for each 2D point measurement: 67 | % 1. Compute measurement predictions from the predicted state by relying on 68 | % the nonlinear measurement model. 69 | % 2. Obtain the corresponding measurement covariance matrix. 70 | % 3. Linearize the measurement model to employ in EKF equations. 71 | predMeas = zeros(curNumMeas * 2, 1); % of the form [x1; y1; z1;...; xn; yn; zn] 72 | measCov = zeros(curNumMeas*2); % measurement noise covariance matrix 73 | linMeasMatrix = zeros(curNumMeas * 2, numStates); % linearized measurement model 74 | for i = 1:curNumMeas 75 | iMeas = transpose(measArray(i, :)); % Select one measurement of the form [xMeas; yMeas] 76 | 77 | % Compute the following variables to exploit in measurement model 78 | diffVector = iMeas - predPos; % The vector from the center to the measurement 79 | diffVectorMag = norm(diffVector); 80 | orientVector = diffVector / diffVectorMag; % The orientation vector (of magnitude 1) 81 | angleGlobal = atan2(diffVector(2), diffVector(1)); % Angle in the global coordinate frame 82 | angleLocal = angleGlobal - predPsi; % Angle in the local coordinate frame 83 | angleLocal = mod(angleLocal, 2*pi); 84 | 85 | covMeasBasis = compute_GP_covariance(angleLocal, basisAngleArray, paramGP); 86 | 87 | H_f = covMeasBasis * inv_P0_extent; % GP model relating the extent and the radial 88 | ... function value evaluated at the current measurement angle 89 | 90 | % Obtain the measurement prediction by the original nonlinear meas model 91 | iPredMeas = predPos + orientVector * H_f * predExtent; % [xPredicted; yPredicted] 92 | predMeas(1+(i-1)*2 : i*2) = iPredMeas; 93 | 94 | % Obtain the covariance of the current measurement set 95 | % Definition: iCovMeas = k(uk,uk), uk: argument of the current measurement 96 | iMeasCov = compute_GP_covariance(angleLocal, angleLocal, paramGP); 97 | % Definition: iR = k(uk,uk) - k(uk, uf) * inv(K(uf,uf)) * k(uf, uk) 98 | iR_f = iMeasCov - covMeasBasis * inv_P0_extent * covMeasBasis'; 99 | measCov(((i-1)*2+1):i*2, ((i-1)*2+1):i*2) = stdMeasGP^2 * eye(2) + ... 100 | orientVector * iR_f * orientVector'; 101 | 102 | % Obtain the linearized model (Details in the Appendix B of the mentioned paper) 103 | dP_dW = ((diffVector*diffVector')./ diffVectorMag^3 - eye(2)./diffVectorMag); 104 | dHf_du = -1/ lengthScaleGP^2 * sin(angleLocal - basisAngleArray') .* covMeasBasis... 105 | * inv_P0_extent; 106 | dThetaG_dw = 1/ diffVectorMag^2 * [diffVector(2) -diffVector(1)]; 107 | 108 | H_lin_pos = eye(2) + dP_dW * (H_f * predExtent)... 109 | + orientVector * dThetaG_dw * (dHf_du * predExtent); 110 | H_lin_psi = -orientVector * dHf_du * predExtent; 111 | H_lin_extent = orientVector * H_f; 112 | 113 | % Linearized measurement model for the current measurement 114 | % = [dh/dxc, dh/dpsi, zeros(due to diff wrt velocities), dh/dextent] 115 | linMeasMatrix(1+(i-1)*2:i*2, :) = [H_lin_pos H_lin_psi zeros(2,3) H_lin_extent]; 116 | end 117 | 118 | % Put the measurements in a column the form: [x1; y1;...; xn; yn] 119 | tempArray = transpose(measArray); 120 | curMeasArrayColumn = tempArray(:); 121 | 122 | % Realize measurement update 123 | kalmanGain = predStateCov * linMeasMatrix' ... 124 | / (linMeasMatrix*predStateCov*linMeasMatrix' + measCov); 125 | estState = predState + kalmanGain * (curMeasArrayColumn - predMeas); 126 | estStateCov = (eye(numStates) - kalmanGain*linMeasMatrix) * predStateCov; 127 | estStateCov = (estStateCov + estStateCov')/2; % To make the covariance matrix symmetric (needed due to numeric errors) 128 | 129 | end 130 | -------------------------------------------------------------------------------- /Matlab/generate_measurements_2D.m: -------------------------------------------------------------------------------- 1 | function [measMatrix, groundTruth] = generate_measurements_2D(measParamaters) 2 | % This function produces point measurements originated from the contour of 3 | % a specified object which is moving according to the specied patter type. 4 | 5 | % Input: 6 | % measParamaters: the parameters utilized 7 | 8 | % Output: 9 | % measMatrix: the measurement matrix comprised of rows 10 | % in the form: [timeStamp xPosition yPosition] 11 | % groundTruth: the ground truth information of the corresponding scenario. 12 | % It is a structure with the following fields: 'objectDescription', 'dataRecord'. 13 | % 'objectDescription' includes object type (e.g., sphere, box...) and 14 | % the parameters of the geometry. 15 | % Each row of the 'dataRecord' is in the form of [timeStamp center' psi] 16 | % where centerPosition = [cX cY]' and psi describes the orientation angle. 17 | 18 | 19 | % Author: Murat Kumru 20 | 21 | 22 | % Extract measurement parameters 23 | experimentDuration = measParamaters{1}; 24 | timeStep = measParamaters{2}; 25 | numMeasPerInstant = measParamaters{3}; 26 | stdMeas = measParamaters{4}; 27 | objType = measParamaters{5}; 28 | objParameters = measParamaters{6}; 29 | motionType = measParamaters{7}; 30 | isPartiallyObserved = measParamaters{8}; 31 | 32 | numIterations = ceil(experimentDuration/ timeStep); 33 | measMatrix = zeros(numIterations*numMeasPerInstant, 3); 34 | gtDataLog = zeros(numIterations, 4); % : [timeStamp centerPosition' psi] 35 | 36 | positionObj = [0 0]; % Initialize the center position of the object 37 | orientationObj = -pi + 2* pi * rand; % Randomly select a constant orientation 38 | for i = 1:numIterations 39 | 40 | curTime = (i-1)*timeStep; 41 | % Simulate the motion of the object 42 | switch motionType 43 | case 1 % No movement 44 | velocityObj = [0 0]; 45 | 46 | case 2 % Constant velocity 47 | velocityObj = [0.3 0.2]; 48 | 49 | case 3 % Sinusoidal motion 50 | velocityObj = [0.05 sin(0.01*2*pi*curTime)]; 51 | end 52 | positionObj = positionObj + timeStep * velocityObj; 53 | 54 | % Produce contour measurements regarding the center 55 | curMeasArray = produceContourMeas(positionObj, orientationObj, numMeasPerInstant, stdMeas, objType,... 56 | objParameters, isPartiallyObserved); 57 | 58 | % Log the groundtruth 59 | gtDataLog(i, :) = [curTime positionObj orientationObj]; 60 | 61 | % Insert the measurements to the output matrix with relevant time stamp 62 | measMatrix((i-1)*numMeasPerInstant + 1 : (i*numMeasPerInstant), :) = ... 63 | [ones(numMeasPerInstant, 1)*curTime curMeasArray]; 64 | end 65 | 66 | groundTruth = struct('objectDescription', [objType, objParameters]... 67 | , 'dataLog', gtDataLog); 68 | end 69 | 70 | 71 | function [posArray] = produceContourMeas(positionObj, orientationObj, numMeasurements, stdMeas, objType... 72 | , objParameters, isPartiallyObserved) 73 | switch objType 74 | case 1 % Circle 75 | radius = objParameters; 76 | 77 | if isPartiallyObserved 78 | thetaArray = pi+ pi * rand(numMeasurements, 1); 79 | rhoArray = radius + stdMeas * randn(numMeasurements, 1); 80 | else 81 | thetaArray = 2*pi * rand(numMeasurements, 1); 82 | rhoArray = radius + stdMeas * randn(numMeasurements, 1); 83 | end 84 | 85 | [xArray, yArray] = pol2cart(thetaArray, rhoArray); 86 | 87 | case 2 % Square 88 | edgeLen = objParameters; 89 | 90 | % First, produce a random array for the selection of the edges 91 | if isPartiallyObserved 92 | edgeArray = randi([1 3], numMeasurements, 1); % Upper vertex is not sampled 93 | else 94 | edgeArray = randi([0 3], numMeasurements, 1); 95 | end 96 | 97 | % Initialize the outputs 98 | xArray = zeros(numMeasurements, 1); 99 | yArray = zeros(numMeasurements, 1); 100 | pointer = 1; 101 | for iEdge = 0:3 102 | num = sum(edgeArray == iEdge); 103 | switch iEdge 104 | case 0 % variable X; Y is set to positive edge 105 | xArray(pointer:(pointer+num-1)) = -edgeLen/2 + edgeLen*rand(num, 1); 106 | yArray(pointer:(pointer+num-1)) = edgeLen/2*ones(num,1) + stdMeas * randn(num, 1); 107 | 108 | case 1 % variable X; Y is set to negative edge 109 | xArray(pointer:(pointer+num-1)) = -edgeLen/2 + edgeLen*rand(num, 1); 110 | yArray(pointer:(pointer+num-1)) = -edgeLen/2*ones(num,1) + stdMeas * randn(num, 1); 111 | 112 | case 2 % variable Y; X is set to positive edge 113 | xArray(pointer:(pointer+num-1)) = edgeLen/2*ones(num,1) + stdMeas * randn(num, 1); 114 | if isPartiallyObserved 115 | yArray(pointer:(pointer+num-1)) = -edgeLen/2 + edgeLen/2*rand(num, 1); 116 | else 117 | yArray(pointer:(pointer+num-1)) = -edgeLen/2 + edgeLen*rand(num, 1); 118 | end 119 | 120 | case 3 % variable Y; X is set to negative edge 121 | xArray(pointer:(pointer+num-1)) = -edgeLen/2*ones(num,1) + stdMeas * randn(num, 1); 122 | if isPartiallyObserved 123 | yArray(pointer:(pointer+num-1)) = -edgeLen/2 + edgeLen/2*rand(num, 1); 124 | else 125 | yArray(pointer:(pointer+num-1)) = -edgeLen/2 + edgeLen*rand(num, 1); 126 | end 127 | end 128 | pointer = pointer + num; 129 | end 130 | 131 | case 3 % Triangle 132 | % It is assumed to be a isosceles triangle 133 | sideEdgeLength = objParameters(1); 134 | bottomEdgeLength = objParameters(2); 135 | height = sqrt(sideEdgeLength^2-(bottomEdgeLength/2)^2); 136 | 137 | % Initialize the outputs 138 | xArray = zeros(numMeasurements, 1); 139 | yArray = zeros(numMeasurements, 1); 140 | for j = 1:numMeasurements 141 | if rand < bottomEdgeLength/ (bottomEdgeLength+2*sideEdgeLength) 142 | % Measurement from bottom edge 143 | if isPartiallyObserved 144 | xArray(j) = -1/3*height + stdMeas*randn; 145 | yArray(j) = (bottomEdgeLength*rand - bottomEdgeLength/2); 146 | else 147 | xArray(j) = -1/3*height + stdMeas*randn; 148 | yArray(j) = (bottomEdgeLength*rand - bottomEdgeLength/2); 149 | end 150 | else 151 | % Measurement from one of the side edges 152 | if isPartiallyObserved 153 | xArray(j) = (height*rand -1/3*height); 154 | yArray(j) = -bottomEdgeLength/2*(1-(xArray(j)/height + 1/3)) + stdMeas*randn; 155 | else 156 | xArray(j) = (height*rand -1/3*height); 157 | randomSign = (rand > 0.5)*2 - 1; 158 | yArray(j) = randomSign * bottomEdgeLength/2*(1-(xArray(j)/height + 1/3)) + stdMeas*randn; 159 | end 160 | end 161 | end 162 | end 163 | 164 | % Compute the rotation matrix 165 | rotMatrix = [cos(orientationObj) -sin(orientationObj); sin(orientationObj) cos(orientationObj)]; 166 | % Rotate measurements 167 | rotXYArray = rotMatrix * [xArray'; yArray']; 168 | 169 | xArray = rotXYArray(1, :)'; 170 | yArray = rotXYArray(2, :)'; 171 | 172 | 173 | posArray = [xArray+positionObj(1) yArray+positionObj(2)]; 174 | end 175 | 176 | -------------------------------------------------------------------------------- /Matlab/visualize_tracking_outputs.m: -------------------------------------------------------------------------------- 1 | function [] = visualize_tracking_outputs(measurements, estState, estStateCov, groundTruth, extentAnglesLocal, time) 2 | 3 | % Extract relevant information from the state 4 | estPos = estState(1:2); 5 | estPsi = estState(3); 6 | estExtent = estState(7:end); 7 | 8 | % Extract the variance of the extent 9 | stdState = sqrt(diag(estStateCov)); 10 | stdExtent = stdState(7:end); 11 | 12 | % Extract ground truth values of the position and the orientation angle 13 | objType = groundTruth.objectDescription(1); 14 | objParam = groundTruth.objectDescription(2:end); 15 | gtKinematics = groundTruth.dataLog(abs(groundTruth.dataLog(:,1)-time)<1e-10, 2:end); 16 | gtCenter =gtKinematics(1:2)'; 17 | gtPsi = gtKinematics(3); 18 | 19 | extentAnglesGlobal = extentAnglesLocal + estPsi; % Calculate angles of the extent in global frame 20 | 21 | cla(); % Clear the current axis 22 | 23 | %% Plot the ground truth 24 | switch objType 25 | case 1 % Circle 26 | plot_circle(gtCenter, objParam, [0.4 0.7 0.3]); 27 | case 2 % Square 28 | plot_square(gtCenter, gtPsi, objParam, [0.4 0.7 0.3]); 29 | case 3 % Triangle 30 | plot_triangle(gtCenter, gtPsi, objParam, [0.4 0.7 0.3]); 31 | end 32 | 33 | 34 | %% Plot measurements 35 | plot(measurements(:,1), measurements(:,2), 'rx', 'LineWidth', 2); 36 | 37 | %% Plot the center position 38 | plot(estPos(1), estPos(2), 'k+', 'LineWidth', 2); 39 | 40 | %% Plot the heading by a line 41 | plot([estPos(1) estPos(1)+3*cos(estPsi)], [estPos(2) estPos(2)+3*sin(estPsi)], 'k', 'LineWidth', 2); 42 | 43 | %% Plot the estimated circumference 44 | [xEstimated, yEstimated] = pol2cart(extentAnglesGlobal, estExtent); 45 | xEstimated = xEstimated + estPos(1); % Shift to the estimated center position 46 | yEstimated = yEstimated + estPos(2); % Shift to the estimated center position 47 | 48 | xEstimatedPlot = [xEstimated; xEstimated(1)]; % To make it appear as a enclosed polygon 49 | yEstimatedPlot = [yEstimated; yEstimated(1)]; % To make it appear as a enclosed polygon 50 | plot(xEstimatedPlot, yEstimatedPlot, 'Color', [0 0.4 0.9], 'LineWidth', 2); 51 | 52 | %% Plot the confidence region of 1-std of the extent 53 | % The uncertainity will be represented with a filled polygon 54 | % Compute the vertices of the inner polygon 55 | [xInner, yInner] = pol2cart(extentAnglesGlobal, max(estExtent - stdExtent, 0)); % Extent can not be smaller than 0 56 | xInner = xInner + estPos(1); % Shift to the estimated center position 57 | yInner = yInner + estPos(2); % Shift to the estimated center position 58 | 59 | % Compute the vertices of the outer polygon 60 | [xOuter, yOuter] = pol2cart(extentAnglesGlobal, (estExtent + stdExtent)); 61 | xOuter = xOuter + estPos(1); % Shift to the estimated center position 62 | yOuter = yOuter + estPos(2); % Shift to the estimated center position 63 | 64 | % Prepare the arrays for plotting 65 | xInnerPlot = [xInner; xInner(1)]; % To make it appear as a enclosed polygon 66 | yInnerPlot = [yInner; yInner(1)]; 67 | xOuterPlot = [xOuter; xOuter(1)]; 68 | yOuterPlot = [yOuter; yOuter(1)]; 69 | 70 | hFill = fill([xOuterPlot; xInnerPlot] , [yOuterPlot; yInnerPlot], 'b'); 71 | hFill.FaceColor = [0.6 0.7 1]; 72 | hFill.EdgeColor = 'none'; 73 | hFill.FaceAlpha = 0.5; 74 | uistack(hFill,'bottom'); 75 | 76 | drawnow(); 77 | 78 | end 79 | 80 | 81 | function plot_circle(center, objectParameters, color) 82 | % Plots a circle 83 | 84 | numControlPoints = 100; 85 | controlAngleArray = transpose(linspace(0, 2*pi, numControlPoints)); 86 | 87 | % Extract object parameters 88 | radius = objectParameters;% Ellipsoid 89 | 90 | % Produce cartesian points on the unit sphere 91 | [xCircle, yCircle] = pol2cart(controlAngleArray, radius*ones(numControlPoints)); 92 | 93 | % Translate these points 94 | xCircle = xCircle+ center(1); 95 | yCircle = yCircle + center(2); 96 | 97 | plot(xCircle, yCircle, 'Color', color, 'LineWidth', 2); 98 | end 99 | 100 | 101 | function plot_square(center, psi, objectParameters, color) 102 | % Plots a square 103 | 104 | % Extract object parameters 105 | edgeLen = objectParameters(1); 106 | 107 | % Define the vertices of the box 108 | vertices_L = [-edgeLen -edgeLen; 109 | edgeLen -edgeLen; 110 | edgeLen edgeLen; 111 | -edgeLen edgeLen; 112 | -edgeLen -edgeLen] * 0.5; 113 | 114 | % Compute the rotation matrix regarding the orientation angle 115 | rotMatrix = compute_rotation_matrix(psi); 116 | 117 | % Transform the local points into the global frame 118 | vertices_G = transpose(rotMatrix * vertices_L'); % Rotation 119 | vertices_G = [vertices_G(:,1)+center(1), vertices_G(:,2)+center(2)]; % Translation 120 | 121 | plot(vertices_G(:,1), vertices_G(:,2), 'Color', color, 'LineWidth', 2); 122 | end 123 | 124 | 125 | function plot_triangle(center, psi, objectParameters, color) 126 | % Plots a triangle 127 | 128 | % Extract object parameters 129 | sideEdgeLength = objectParameters(1); 130 | bottomEdgeLength =objectParameters(2); 131 | height = sqrt(sideEdgeLength^2-(bottomEdgeLength/2)^2); 132 | 133 | % Define the vertices of the box 134 | vertices_L = [-height/3 -bottomEdgeLength/2; 135 | 2/3*height 0; 136 | -height/3 bottomEdgeLength/2; 137 | -height/3 -bottomEdgeLength/2]; 138 | 139 | % Compute the rotation matrix regarding the orientation angle 140 | rotMatrix = compute_rotation_matrix(psi); 141 | 142 | % Transform the local points into the global frame 143 | vertices_G = transpose(rotMatrix * vertices_L'); % Rotation 144 | vertices_G = [vertices_G(:,1)+center(1), vertices_G(:,2)+center(2)]; % Translation 145 | 146 | plot(vertices_G(:,1), vertices_G(:,2), 'Color', color, 'LineWidth', 2); 147 | end 148 | 149 | 150 | function rotationMatrix = compute_rotation_matrix(psi) 151 | rotationMatrix = [cos(psi) -sin(psi); sin(psi) cos(psi)]; 152 | end 153 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | This repository presents an example implementation of the algorithm proposed in the following paper. 2 | 3 | "Extended target tracking using Gaussian processes" (IEEE Trans. on Signal Process., vol. 63, no. 16, pp. 4165–4178, Apr. 2015) by N. Wahlström and E Özkan 4 | DOI: 10.1109/TSP.2015.2424194 5 | URL: https://ieeexplore.ieee.org/abstract/document/7088657 6 | --------------------------------------------------------------------------------