├── GPETT
    ├── compute_GP_covariance.m
    ├── demo.m
    ├── filter_GPETT3D.m
    ├── get_measurements.m
    ├── rotation_matrix_from_global_to_local.m
    └── visualize_tracking_outputs.m
├── GPETTP
    ├── compute_GP_covariance.m
    ├── demo.m
    ├── filter_GPETT3DProjection.m
    ├── get_measurements.m
    ├── reconstruct_from_projections.m
    ├── rotation_matrix_from_global_to_local.m
    └── visualize_tracking_outputs.m
└── README.md


/GPETT/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 | % Output:
  4 | %           covMatrix:      The covariance matrix computed by the GP kernel.
  5 | % Inputs:
  6 | %           argArray1:       Each row is a pair of spherical angles, i.e., = [azimuth elevation]
  7 | %           argArray2:       Each row is a pair of spherical angles, i.e., = [azimuth elevation]
  8 | %           paramGP:         Parameters of the underlying GP
  9 | 
 10 | % Extract parameters
 11 | kernelType = paramGP{1};
 12 | stdPrior = paramGP{2};
 13 | stdRadius  = paramGP{3};
 14 | scaleLength = paramGP{4};
 15 | isObjectSymmetric = paramGP{5};
 16 | 
 17 | switch kernelType
 18 |     
 19 |     case 1
 20 |         % Periodic squared exponential kernel               
 21 |         diffMatrix = compute_diffence_matrix(argArray1, argArray2);
 22 |         
 23 |         if isObjectSymmetric
 24 |             % Periodic with pi (it imposes the symmetry of the object)
 25 |             covMatrix = stdPrior^2 * exp(-sin(diffMatrix).^2 / (2*scaleLength^2)) + stdRadius^2;
 26 |         else
 27 |             % Covariance periodic with 2pi
 28 |             covMatrix = stdPrior^2 * exp(-diffMatrix.^2 / (2*scaleLength^2)) + stdRadius^2;
 29 |         end
 30 |         
 31 |     case 2
 32 |         % Matern 3_2 kernel
 33 |         diffMatrix = compute_diffence_matrix(argArray1, argArray2);        
 34 |         covMatrix = stdPrior^2 * (1+sqrt(3)/scaleLength*diffMatrix) .* exp(-sqrt(3)/scaleLength*diffMatrix);
 35 |         
 36 |     case 3
 37 |         % Matern 5_2 kernel
 38 |         diffMatrix = compute_diffence_matrix(argArray1, argArray2);        
 39 |         covMatrix = stdPrior^2 * (1+sqrt(5)/scaleLength*diffMatrix...
 40 |             + 5/(3*scaleLength^2)*diffMatrix.^2) .* exp(-sqrt(5)/scaleLength*diffMatrix);
 41 | end
 42 | end
 43 | 
 44 | 
 45 | function [ diffMatrix ] = compute_diffence_matrix( inp1, inp2 )
 46 | % This function calculates the difference matrix regarding two inputs.
 47 | % inp1, inp2: Each row is of the form [azimuthAngle elevationAngle]
 48 | 
 49 | % Produce a grid structure to be able to compute each angle between
 50 | % elements of two inputs.
 51 | len1 = size(inp1, 1); % The number of spherical angles, : [azi_1 elv_1;... azi_N elv_N]
 52 | len2 = size(inp2, 1); % The number of spherical angles: : [azi_1 elv_1;... azi_N elv_N]
 53 | 
 54 | % Extract azimuth and elevation angles from the inputs
 55 | aziArray1 = inp1(:,1);
 56 | elvArray1 = inp1(:,2);
 57 | aziArray2 = inp2(:,1);
 58 | eleArray2 = inp2(:,2);
 59 | 
 60 | % Create matrices from these vectors to form grid structure
 61 | aziGrid1 = repmat(aziArray1, [1, len2]);
 62 | elvGrid1 = repmat(elvArray1, [1, len2]);
 63 | aziGrid2 = repmat(transpose(aziArray2), [len1, 1]);
 64 | elvGrid2 = repmat(transpose(eleArray2), [len1, 1]);
 65 | 
 66 | % Put the matrices into vectors to feed to the angle computation procedure
 67 | aziArray1 = aziGrid1(:);
 68 | elvArray1 = elvGrid1(:);
 69 | aziArray2 = aziGrid2(:);
 70 | eleArray2 = elvGrid2(:);
 71 | 
 72 | % Prepare the arrays for the following function
 73 | sphrCoorArray1 = [aziArray1 elvArray1];
 74 | sphrCoorArray2 = [aziArray2 eleArray2];
 75 | 
 76 | % Calculate the the difference (spherical angle)
 77 | diffArray = compute_angle_btw_sphr_coor(sphrCoorArray1, sphrCoorArray2);
 78 | 
 79 | % Transform difference array back in matrix form
 80 | diffMatrix = reshape(diffArray, len1, len2);
 81 | end
 82 | 
 83 | function [ angle ] = compute_angle_btw_sphr_coor( sphrCoor1, sphrCoor2 )
 84 | % The function computes the angle between two Spherical coordinates.
 85 | % sphrCoor1, sphrCoor2 are in the form of : [azimuthAngle elevationAngle]. Note that
 86 | % azimuthAngle and elevation angle can also be column vectors.
 87 | 
 88 | azi1 = sphrCoor1(:, 1);
 89 | elv1 = sphrCoor1(:, 2);
 90 | azi2 = sphrCoor2(:, 1);
 91 | elv2 = sphrCoor2(:, 2);
 92 | 
 93 | % Calculate trigonometric multiplications
 94 | multCosAzi = cos(azi1) .* cos(azi2);
 95 | multSinAzi = sin(azi1) .* sin(azi2);
 96 | multSinElv = sin(elv1) .* sin(elv2);
 97 | multCosElv = cos(elv1) .* cos(elv2);
 98 | 
 99 | angle = real(acos(multCosElv.*multCosAzi + multCosElv.*multSinAzi + multSinElv));
100 | end
101 | 


--------------------------------------------------------------------------------
/GPETT/demo.m:
--------------------------------------------------------------------------------
  1 | % This file presents a demonstration of the extended tracker proposed in 
  2 | % Section V of the following study.
  3 | 
  4 | % For the details, please see:
  5 | %       M. Kumru and E. Özkan, “Three-Dimensional Extended Object Tracking 
  6 | %       and Shape Learning Using Gaussian Processes”, 
  7 | %       IEEE Transactions on Aerospace and Electronic Systems, 2021.
  8 | %       https://ieeexplore.ieee.org/abstract/document/9382878/
  9 | 
 10 | % Dependencies:
 11 | %       "Spheretri" package by Peter Gagarinov
 12 | %       It is utilized to produce uniformly distributed points on the unit sphere.
 13 | %       https://www.mathworks.com/matlabcentral/fileexchange/58453-spheretri
 14 | 
 15 | clc;
 16 | clear;
 17 | close all;
 18 | 
 19 | 
 20 | %% Paramaters
 21 | % Simulation Parameters
 22 | expDuration = 30;   % (in seconds)
 23 | T = 0.1;            % sampling time (in seconds)
 24 | eps = 1e-6;         % a small scalar
 25 | 
 26 | % Measurement Parameters
 27 | numMeasPerInstant = 20;
 28 | paramMeas = {numMeasPerInstant, expDuration, T};
 29 | 
 30 | % Gaussian Process (GP) Parameters
 31 | minNumBasisAngles = 200;        % minimum number of basis angles on which the extent is maintained
 32 | meanGP = 0;                     % mean of the GP
 33 | stdPriorGP = 1;                 % prior standard deviation of the GP
 34 | stdRadiusGP = 0.2;              % standard deviation of the radius
 35 | scaleLengthGP = pi/8;           % lengthscale
 36 | kernelTypeGP = 1;               % 1:Squared exponential , 2: Matern3_2, 3: Matern5_2
 37 | isObjSymmetric = 0;             % it is used to adjust the kernel function accordingly
 38 | stdMeasGP = 0.1;                % standard deviation of the measurements (used in the GP model)
 39 | paramGP = {kernelTypeGP, stdPriorGP, stdRadiusGP, scaleLengthGP, isObjSymmetric, stdMeasGP, meanGP};
 40 | 
 41 | % EKF Paramaters
 42 | stdCenter = 1e-1;               % std dev of the process noise for object center
 43 | stdAngVel = 1e-1;               % std dev of the process noise for angular velocity
 44 | lambda = 0.99;                   
 45 | 
 46 | % Determine the Basis Angles
 47 | [basisVertices, ~] = spheretri(minNumBasisAngles);      % produces evenly spaced points on the sphere
 48 | [azimuthBasisArray, elevationBasisArray, ~] = cart2sph(basisVertices(:,1), basisVertices(:,2)...
 49 |     , basisVertices(:,3));
 50 | azimuthBasisArray = mod(azimuthBasisArray, 2*pi);       % to make it consistent with the convention
 51 | numBasisAngles = size(basisVertices, 1);
 52 | % Arrange the basis array so that azimuth and elevation are in ascending order
 53 | basisAngleArray = [azimuthBasisArray elevationBasisArray];
 54 | basisAngleArray = sortrows(basisAngleArray, 2);
 55 | basisAngleArray = sortrows(basisAngleArray, 1);
 56 | 
 57 | 
 58 | %% Load Measurements and Ground Truth 
 59 | [measComplete, groundTruth] = get_measurements(paramMeas);
 60 | 
 61 | 
 62 | %% Specify the Initial Distribution
 63 | % Initial covariance
 64 | P0_c = 0.1 * eye(3);
 65 | P0_v = 0.1 * eye(3);
 66 | P0_a = 1e-5 * eye(3,3);
 67 | P0_w = 1e-0 * eye(3,3);
 68 | P0_extent = compute_GP_covariance(basisAngleArray, basisAngleArray, paramGP);  
 69 | P0 = blkdiag(P0_c, P0_v, P0_a, P0_w, P0_extent);
 70 | 
 71 | % Initial mean
 72 | c0 = groundTruth.dataLog(1, 2:4)'+ sqrt(0.1)*randn(3,1);        % initial linear velocity
 73 | v0 = [0.5 0 0]'+ sqrt(0.1)*randn(3,1);                          % initial linear velocity
 74 | q0 = [0 0 0 1]';                                                % initial quaternions 
 75 | a0 = [0 0 0]';                                                  % initial orientation deviation
 76 | w0 = [0 0 0]';                                                  % initial angular velocity
 77 | f0 = meanGP * ones(numBasisAngles, 1);                          % initial extent (is initialized regarding the GP model)
 78 | 
 79 | % Initialize the filter estimate
 80 | estState = [c0; v0; a0; w0; f0];
 81 | estQuat = q0;
 82 | estStateCov = P0;
 83 | 
 84 | 
 85 | %% Define the Process Model
 86 | F_lin = kron([1 T; 0 1], eye(3));       % constant velocity model for linear motion
 87 | F_rot = eye(6,6);                       % a substitute matrix for the rotational dynamics (it will be computed in the filter at each recursion)
 88 | F_f = eye(numBasisAngles);      
 89 | F = blkdiag(F_lin, F_rot, F_f);
 90 | 
 91 | Q_lin = kron([T^3/3 T^2/2; T^2/2 T], stdCenter^2*eye(3));   % process covariance of linear motion
 92 | Q_rot = eye(6,6);                                           % a substitute matrix for the covariance of the rotational motion (it will be computed in the filter at each recursion)
 93 | Q_extent = zeros(numBasisAngles);                           % predicted covariance of the extent will be computed within the filter according to Eqn. 20 
 94 | Q = blkdiag(Q_lin, Q_rot, Q_extent);
 95 | 
 96 | processModel = {F, Q, lambda};
 97 | 
 98 |  
 99 | %% Open a Figure (to illustrate the tracking outputs)
100 | figure;
101 | grid on; hold on; axis equal;
102 | view(45, 25);
103 | title('Extended Target Tracking Results');
104 | xlabel('X axis');
105 | ylabel('Y axis');
106 | zlabel('Z axis');
107 | 
108 | 
109 | %% GPETT3D LOOP
110 | time = 0;
111 | numInstants = ceil(expDuration/ T);
112 | filterParam = {processModel, paramGP, basisAngleArray, T, stdAngVel, eps}; 
113 | 
114 | for k = 1:numInstants
115 |     
116 |     % Extract current measurements
117 |     curMeasArray = measComplete(abs(measComplete(:, 1)-time)<eps, 2:4);    
118 |     
119 |     % Call the GPETT3D filter
120 |     [estState, estQuat, estStateCov] = filter_GPETT3D(estState, estQuat...
121 |         ,estStateCov, curMeasArray, filterParam);
122 |     
123 |     % Visualize the results
124 |     visualize_tracking_outputs(estState, estQuat, diag(estStateCov), curMeasArray, groundTruth...
125 |         ,time, basisAngleArray);        
126 | 
127 |     % Update time
128 |     time = time + T;
129 | end


--------------------------------------------------------------------------------
/GPETT/filter_GPETT3D.m:
--------------------------------------------------------------------------------
  1 | function [ estState, estQuat, estStateCov ] = filter_GPETT3D( prevEstState...
  2 |     , prevEstQuat, prevEstStateCov, measArray, filterParam)
  3 | 
  4 | % Extract the parameters 
  5 | processModel = filterParam{1};
  6 | paramGP = filterParam{2};
  7 | basisAngleArray = filterParam{3};
  8 | T = filterParam{4};
  9 | stdAngVel = filterParam{5};
 10 | eps = filterParam{6};
 11 | 
 12 | F = processModel{1};
 13 | Q = processModel{2};
 14 | lambda = processModel{3};
 15 | 
 16 | % Compute the inverse of P0_extent since it will repeatedly be used in the system dynamic model
 17 | persistent inv_P0_extent;
 18 | if isempty(inv_P0_extent)
 19 |     P0_extent = compute_GP_covariance(basisAngleArray, basisAngleArray, paramGP);
 20 |     P0_extent = P0_extent + eps*eye(size(basisAngleArray,1));       % to prevent numerical errors thrown in matrix inversion
 21 |     chol_P0_extent = chol(P0_extent);
 22 |     inv_chol_P0_extent = inv(chol_P0_extent);
 23 |     inv_P0_extent = inv_chol_P0_extent * inv_chol_P0_extent';
 24 | end
 25 | 
 26 | 
 27 | %% Process Update
 28 | % Compute the rotational dynamic model
 29 | angVelEst = prevEstState(10:12);
 30 | [FRot, QRot] = compute_rotational_dynamics(angVelEst, stdAngVel, T);
 31 | 
 32 | % Substitute the rotational model
 33 | F(7:12, 7:12) = FRot;
 34 | Q(7:12, 7:12) = QRot;
 35 | 
 36 | % Compute predicted state and covariance
 37 | predState = F * prevEstState;
 38 | predQuat = prevEstQuat;
 39 | predStateCov = F * prevEstStateCov * F' + Q;
 40 | 
 41 | % Dynamic model for maximum entropy in extent
 42 | predStateCov(13:end, 13:end) = 1/lambda * prevEstStateCov(13:end, 13:end);
 43 | 
 44 | %% Measurement Update
 45 | curNumMeas = size(measArray, 1);
 46 | numStates = size(F,1);
 47 | 
 48 | % In the below loop, the following operations are performed for each 3D point measurement:
 49 | % 1. Compute measurement predictions from the predicted state by relying on
 50 | % the nonlinear measurement model.
 51 | % 2. Obtain the corresponding measurement covariance matrix.
 52 | % 3. Linearize the measurement model to employ in EKF equations.
 53 | predMeas = zeros(curNumMeas * 3, 1);                 	% of the form [x1; y1; z1;...; xn; yn; zn]
 54 | measCov = zeros(curNumMeas*3);                         	% measurement noise covariance matrix
 55 | linMeasMatrix = zeros(curNumMeas * 3, numStates);       % linearized measurement model
 56 | for i = 1:curNumMeas
 57 |     iMeas = transpose(measArray(i, :));    % select one measurement
 58 |     
 59 |     % Obtain predicted measurement and covariance
 60 |     [iMeasPredicted,  iMeasCovariance]  = compute_meas_prediction_and_covariance...
 61 |         (iMeas, predState, predQuat, paramGP, basisAngleArray, inv_P0_extent);
 62 |     
 63 |     % Log these variables to later utilize in the update mechanism
 64 |     predMeas(1+(i-1)*3 : i*3) = iMeasPredicted;
 65 |     measCov(((i-1)*3+1):i*3, ((i-1)*3+1):i*3) = iMeasCovariance;
 66 |     
 67 |     % Obtain linearized measurement model
 68 |     iLinMeasMatrix = compute_linearized_measurement_matrix(iMeas, predState...
 69 |         , predQuat, paramGP, basisAngleArray, inv_P0_extent, eps);
 70 |     
 71 |     % Linearized measurement model for the current measurement
 72 |     % linMeasMatrix = [dh/dc, dh/dv = zero, dh/dq, dh/dw = zero, dh/dextent]
 73 |     linMeasMatrix(1+(i-1)*3:i*3, :) = iLinMeasMatrix;
 74 | end
 75 | 
 76 | % Put the measurements in a column the form: [x1; y1; z1;...; xn; yn; zn]
 77 | tempArray = transpose(measArray);
 78 | curMeasArrayColumn = tempArray(:);
 79 | 
 80 | % Realize measurement update
 81 | kalmanGain = predStateCov * linMeasMatrix'...
 82 |     / (linMeasMatrix*predStateCov*linMeasMatrix' + measCov);
 83 | estState = predState + kalmanGain * (curMeasArrayColumn - predMeas);
 84 | estStateCov = (eye(numStates) - kalmanGain*linMeasMatrix) * predStateCov;
 85 | estStateCov = (estStateCov + estStateCov')/2;       % to make the covariance matrix symmetric (needed due to numeric errors)
 86 | 
 87 | % Quaternion Treatment
 88 | estQuatDev = estState(7:9);
 89 | estQuat = apply_quat_deviation(estQuatDev, predQuat);
 90 | estState(7:9) = zeros(3,1);                         % Reset the orientation deviation
 91 | 
 92 | end
 93 | 
 94 | 
 95 | function [measPredicted,   measCovariance] = compute_meas_prediction_and_covariance...
 96 |     (meas, state, quat, paramGP, basisAngleArray, inv_P0_extent)
 97 | 
 98 | meanGP = paramGP{7};    % mean of the GP
 99 | 
100 | % Extract relevant information from the predicted state
101 | center = state(1:3);
102 | extent = state(13:end);
103 | 
104 | % Compute the following variables to exploit in measurement model
105 | [p, H_f, covMeasBasis, angle_L] = compute_measurement_model_matrices...
106 |     (meas, state, quat, paramGP, basisAngleArray, inv_P0_extent);
107 | 
108 | % Obtain the measurement prediction by the original nonlinear model
109 | measPredicted = center + p * H_f * extent + p*meanGP*(1-H_f*ones(size(extent,1),1));
110 | 
111 | % Definition: iCovMeas = k(uk,uk), uk: argument of the current measurement
112 | covGP = compute_GP_covariance(angle_L, angle_L, paramGP);
113 | % Definition: iR = k(uk,uk) - k(uk, uf) * inv(K(uf,uf)) * k(uf, uk)
114 | iR_f = covGP - covMeasBasis * inv_P0_extent * covMeasBasis';
115 | % Obtain the covariance of the measurement
116 | stdMeasGP = paramGP{6};
117 | measCovariance = stdMeasGP^2 * eye(3) + p * iR_f * p';
118 | end
119 | 
120 | function [ linearMeasMat ] = compute_linearized_measurement_matrix( meas, state...
121 |     , quat, paramGP, basisAngleArray, inv_P0_extent, eps)
122 | 
123 | meanGP = paramGP{7};    % mean of the GP
124 | 
125 | %% Obtain the measurement prediction by the original nonlinear meas model
126 | [p, H_f, ~, ~] = compute_measurement_model_matrices(meas, state, quat, paramGP...
127 |     , basisAngleArray, inv_P0_extent);
128 | center = state(1:3);
129 | extent = state(13:end);
130 | measPredicted = center + p * H_f * extent + p*meanGP*(1-H_f*ones(size(extent,1),1));
131 | 
132 | %% Calculate partial derivative wrt target extent
133 | H_extent = p * H_f;     % This partial derivative is taken analytically.
134 | 
135 | %% Calculate partial derivative wrt linear positions
136 | H_center = zeros(3,3);
137 | for i = 1:3
138 |     stateIncremented = state;   % Initialize the vector
139 |     stateIncremented(i) = stateIncremented(i) + eps;    % increment the state
140 |     centerIncremented = stateIncremented(1:3);          % incremented center
141 |     
142 |     % Compute the output of original measurement model for the incremented state vector
143 |     [p, H_f, ~, ~] = compute_measurement_model_matrices(meas, stateIncremented...
144 |         , quat, paramGP, basisAngleArray, inv_P0_extent);
145 |     measPredictedForIncState = centerIncremented + p * H_f * extent + p*meanGP*(1-H_f*ones(size(extent,1),1));
146 |     
147 |     % Compute numeric derivative
148 |     H_center(:, i) = (measPredictedForIncState - measPredicted) / eps;
149 | end
150 | 
151 | %% Calculate partial derivative wrt linear velocities
152 | H_linVel = zeros(3,3); % Measurement model does not depend on linear velocities
153 | 
154 | %% Calculate partial derivative wrt quaternion deviations
155 | H_quatDev = zeros(3,3);
156 | for i = (7:9)
157 |     stateIncremented = state;   % Initialize the vector
158 |     stateIncremented(i) = stateIncremented(i) + eps;
159 |     
160 |     % Compute the output of original measurement model for the incremented state vector
161 |     [p, H_f, ~, ~] = compute_measurement_model_matrices(meas, stateIncremented...
162 |         , quat, paramGP, basisAngleArray, inv_P0_extent);
163 |     measPredictedForIncState = center + p * H_f * extent + p*meanGP*(1-H_f*ones(size(extent,1),1));
164 |     
165 |     % Compute numeric derivative
166 |     H_quatDev(:, i-6) = (measPredictedForIncState - measPredicted) / eps;
167 | end
168 | 
169 | %% Calculate partial derivative wrt angular velocities
170 | H_angVel = zeros(3,3); % Measurement model does not depend on linear velocities
171 | 
172 | %% Form the linearized measurement matrix
173 | linearMeasMat = [H_center H_linVel H_quatDev H_angVel H_extent];
174 | 
175 | end
176 | 
177 | function [diffUnitVector_G, H_f, covMeasBasis, measAngle_L] = compute_measurement_model_matrices...
178 |     (meas, state, quatPrev, paramGP, basisAngleArray, inv_P0_extent)
179 | 
180 | % Extract relevant information from the state
181 | center = state(1:3);
182 | quatDev = state(7:9);
183 | quat = apply_quat_deviation(quatDev, quatPrev); % Apply predicted orientation deviation
184 | 
185 | % Compute the following variables to exploit in measurement model
186 | diffVector_G = meas - center;   % the vector from the center to the measurement
187 | diffVectorMag = norm(diffVector_G);
188 | if diffVectorMag == 0
189 |     diffVectorMag = eps;
190 | end
191 | diffUnitVector_G = diffVector_G / diffVectorMag;
192 | 
193 | % Express the diffVectorGlobal in Local frame
194 | R_from_G_to_L = rotation_matrix_from_global_to_local(quat);
195 | diffVector_L = R_from_G_to_L * diffUnitVector_G;
196 | 
197 | % Find the Spherical angle in Local frame corresponding to this vector
198 | [azi_L, elv_L, ~] = cart2sph(diffVector_L(1), diffVector_L(2), diffVector_L(3));
199 | azi_L = mod(azi_L, 2*pi);       % to keep it between 0 and 2*pi
200 | angle_L = [azi_L elv_L];        % angle in the local coordinate frame
201 | 
202 | % Compute the covariance matrix component from the GP model
203 | covMeasBasis = compute_GP_covariance(angle_L, basisAngleArray, paramGP);
204 | 
205 | H_f = covMeasBasis * inv_P0_extent; % GP model relating the extent and the radial
206 | % function value evaluated at the current measurement angle
207 | 
208 | measAngle_L = angle_L;  % the angle of the measurement in local coordinate frame
209 | 
210 | end
211 | 
212 | function [F, Q] = compute_rotational_dynamics(w, std, T)
213 | % Inputs:
214 | %              w:        Angular rate
215 | %              std:     Standard deviation of the angular velocity
216 | %              T:         Sampling time
217 | 
218 | % Dummy variables
219 | wNorm = norm(w);
220 | if wNorm == 0
221 |     wNorm = 1e-3;
222 | end
223 | 
224 | S = skew_symmetric_matrix(-w);
225 | c = cos(1/2*T*wNorm);
226 | s = sin(1/2*T*wNorm);
227 | expMat = eye(3,3) + s/wNorm*S + (1-c)/wNorm^2*S^2;
228 | 
229 | % Construct the state transition matrix
230 | F = [expMat  T*expMat ; zeros(3,3)  eye(3,3)];
231 | 
232 | % Construct the process noise covariance matrix
233 | G11 = T*eye(3,3) + 2/wNorm^2*(1-c)*S + 1/wNorm^2*(T-2/wNorm*s)*S^2;
234 | G12 = 1/2*T^2*eye(3,3) + 1/wNorm^2*(4/wNorm*s-2*T*c)*S + 1/wNorm^2*(1/2*T^2+2/wNorm*T*s+4/wNorm^2*(c-1))*S^2;
235 | G21 = zeros(3,3);
236 | G22 = T * eye(3,3);
237 | G = [G11 G12; G21 G22];
238 | 
239 | B = G * [zeros(3,3); eye(3,3)];
240 | 
241 | % cov = std^2 * diag([0 0 1]);
242 | cov = std^2 * eye(3,3);
243 | Q = B * cov * B';
244 | end
245 | 
246 | function [out] = skew_symmetric_matrix(x)
247 | out = [0  -x(3)  x(2);...
248 |     x(3)  0  -x(1);...
249 |     -x(2)  x(1)  0];
250 | end
251 | 
252 | function [qOut] = apply_quat_deviation(a, qIn)
253 | qa = 1/ sqrt(4+norm(a)) * [a; 2];   % It depends on the Rodrigues parametrization
254 | qOut = quat_product(qa, qIn);
255 | 
256 | qOut = qOut/ norm(qOut);    % Normalize due to numeric inprecisions
257 | end
258 | 
259 | function [out] = quat_product(q1, q2)
260 | q1V = q1(1:3);
261 | q1S = q1(4);
262 | q2V = q2(1:3);
263 | q2S = q2(4);
264 | 
265 | out = [q1S*q2V + q2S*q1V - cross(q1V,q2V);...
266 |     q1S*q2S - q1V'*q2V];
267 | end
268 | 


--------------------------------------------------------------------------------
/GPETT/get_measurements.m:
--------------------------------------------------------------------------------
  1 | function [ measComplete, groundTruth ] = get_measurements( paramMeas )
  2 | %GET_MEASUREMENTS provides point measurements. 
  3 | % Outputs:
  4 | %	MeasComplete: is the complete set of point measurements. Each row is in
  5 | %       the form of [timeStamp x y z]
  6 | %   groundTruth: is the ground truth information of the corresponding scenario.
  7 | %       It is a structure with the following fields: 'objectDescription', 'dataRecord'.
  8 | %       'objectDescription' includes type (e.g., sphere, box...) and the parameters
  9 | %       of the geometry.
 10 | %       Each row of the 'dataRecord' is in the form of [timeStamp center' quaternions']
 11 | %       where centerPosition = [cX cY cZ]' and quaternions = [q0 q1 q2 q3]'.
 12 | 
 13 | % Input:
 14 | %   paramMeas: measurement parameters. It is cell variable including the
 15 | %       following information {measSource, numMeasPerInstant, expDuration, T}
 16 | 
 17 | % Extract the parameters
 18 | numMeasPerInstant = paramMeas{1};
 19 | expDuration = paramMeas{2};
 20 | T = paramMeas{3};
 21 | numInstants = ceil(expDuration/ T);
 22 | stdMeas = 0.1;              % Measurement noise std
 23 | 
 24 | % Object parameters
 25 | objType = 2; % objType = 1:Sphere, 2:Box
 26 | switch objType
 27 |     case 1 % Sphere
 28 |         radius = 2;
 29 |         objParameters = radius;
 30 |     case 2 % Box
 31 |         l = 3;      % Length
 32 |         h = 3;      % Height
 33 |         w = 3;      % Width
 34 |         objParameters = [l h w];
 35 | end
 36 | 
 37 | % Generate the measurements in Matlab
 38 | [measComplete, gtDataLog] = generate_measurements_Matlab...
 39 |     (stdMeas, numMeasPerInstant, objType, objParameters, numInstants, T);
 40 | 
 41 | groundTruth = struct('objectDescription', [objType, objParameters]...
 42 |     , 'dataLog', gtDataLog);
 43 | 
 44 | end
 45 | 
 46 | function [surfaceMeasurements, groundTruth] = generate_measurements_Matlab...
 47 |     (stdMeas, numMeasPerInstant, objType, objParameters, numInstants, T)
 48 | % This function produces surface measurements from a 3D object.
 49 | % Outputs:
 50 | %       surfaceMeasurements = [timeStamp xPosition yPosition zPosition]
 51 | %       groundTruth = [timeStamp centerPosition' quaternions']
 52 | %           where centerPosition = [cX cY cZ] and quaternions = [q0 q1 q2 q3]
 53 | % Inputs:
 54 | %       stdMeas: standart deviation of a 3D point measurement on each axis
 55 | %       numInstants: number of instants included in the simulation
 56 | %       objType: enumerated variable, 1:Sphere, 2:Cube, 3:Ellipsoid,...
 57 | %       objParameters: depends on the object, i.e., for Cube it denotes edgeLength
 58 | %       experimentDuration: in seconds
 59 | %       T: time step between successive siulation instants (in seconds)
 60 | 
 61 | % Comment out one of the test scenarios below (A or B)
 62 | % A. Straight-line pattern:
 63 | % initialQuaternion = [0 ; 0; 0; 1];
 64 | % initialPosition = [0; 0; 0];
 65 | % linearVelocityMagnitude = 10; % in m/sec
 66 | % angularVelocityArray = zeros(numInstants, 3);
 67 | % angularVelocityPhi = 0;         % in rad/sec, around X axis of the local frame
 68 | % angularVelocityTheta = 0;   % in rad/sec, around Y axis of the local frame
 69 | % angularVelocityPsi = 0;    % in rad/sec, around Z axis of the local frame
 70 | % angularVelocityArray(:, 1) = angularVelocityPhi; 
 71 | % angularVelocityArray(:, 2) = angularVelocityTheta; 
 72 | % angularVelocityArray(31:230, 3) = angularVelocityPsi; 
 73 | 
 74 | % B. U-turn pattern:
 75 | initialQuaternion = [0; 0; 0; 1];
 76 | initialPosition = [0; 0; 0];
 77 | linearVelocityMagnitude = 0.5;          % in m/sec
 78 | angularVelocityArray = zeros(numInstants, 3);
 79 | angularVelocityPhi = pi/10;             % in rad/sec, around X axis of the local frame
 80 | angularVelocityTheta = pi/15;           % in rad/sec, around Y axis of the local frame
 81 | angularVelocityPsi = pi/20;             % in rad/sec, around Z axis of the local frame
 82 | angularVelocityArray(30:100, 1) = -angularVelocityPhi; 
 83 | angularVelocityArray(170:220, 1) = 0.7*angularVelocityPhi; 
 84 | angularVelocityArray(50:120, 2) = angularVelocityTheta; 
 85 | angularVelocityArray(190:250, 2) = -1*angularVelocityTheta; 
 86 | angularVelocityArray(80:150, 3) = angularVelocityPsi; 
 87 | angularVelocityArray(220:260, 3) = 0*angularVelocityPsi; 
 88 | 
 89 | %% Simulate the Motion
 90 | % Note that only kinematic aspects of motion is considered. No dynamic behaviour is introduced.
 91 | posArray = zeros(numInstants, 3); % : [xArray yArray zArray]
 92 | quatArray = zeros(numInstants, 4); % : [q0Array q1Array q2Array q3Array]
 93 | velArray = zeros(numInstants, 3); 
 94 | groundTruth = zeros(numInstants, 14); % : [timeStamp centerPosition' quaternions' angularRate' linearVel']
 95 | 
 96 | pos_k = initialPosition; % Initialize position
 97 | quat_k = normc(initialQuaternion); % Initialize quaternions
 98 | vel_k = linearVelocityMagnitude * [1; 0; 0];
 99 | angRate_k = angularVelocityArray(1,:)';
100 | 
101 | % Record the initial state
102 | posArray(1, :) = pos_k';
103 | velArray(1, :) = vel_k';
104 | quatArray(1, :) = quat_k';
105 | groundTruth(1, :) = [0 pos_k' quat_k' angRate_k' vel_k'];
106 | 
107 | for k = 1:(numInstants-1)
108 |     curTime = k*T;
109 |     
110 |     % Obtain transformation matrices
111 |     % First, find the linear velocity rotation matrix from local frame to the global
112 |     R_from_G_to_L = rotation_matrix_from_global_to_local(quat_k);
113 |     R_from_L_to_G = transpose(R_from_G_to_L);
114 |     % Then, find the matrix to transform local angular velocities into quaternion diffential
115 |     T_from_L_to_G = angular_velocity_transformation_matrix_from_local_to_global(quat_k);
116 |     
117 |     % Obtain the linear velocity vector expressed in the local frame
118 |     v_L_k = linearVelocityMagnitude * [1; 0; 0]; % It is assumed to be in X direction
119 |     
120 |     % Extract the current angular velocity vector
121 |     w_L_k = transpose(angularVelocityArray(k, :));
122 |     
123 |     % Convert the velocities into global frame
124 |     posDot_G_k = R_from_L_to_G * v_L_k;
125 |     %     quatDot_G_k = T_from_L_to_G * w_L_k;
126 |     quatDot_G_k = 1/2 * [quat_k(4)*w_L_k - cross(w_L_k, quat_k(1:3)); -w_L_k'*quat_k(1:3)];
127 |     
128 |     % Update the position and quaternions
129 |     pos_k = pos_k + T * posDot_G_k;
130 |     quat_k = quat_k + T * quatDot_G_k;
131 |     quat_k = normc(quat_k);
132 |     
133 |     % Record the variables
134 |     posArray(k+1, :) = pos_k';
135 |     velArray(k+1, :) = posDot_G_k';
136 |     quatArray(k+1, :) = quat_k';
137 |     groundTruth(k+1, :) = [curTime pos_k' quat_k' angularVelocityArray(k,:) posDot_G_k'];
138 | end
139 | 
140 | %% Produce surface measurements
141 | surfaceMeasurements = zeros(numInstants*numMeasPerInstant, 4); % Initialize the measurements
142 | for i = 1:numInstants
143 |     iTime = groundTruth(i, 1);
144 |     iPos = groundTruth(i, 2:4)';
145 |     iQuat = groundTruth(i, 5:8)';
146 |     
147 |     curMeasArray = produce_surface_measurements(iPos, iQuat, numMeasPerInstant, stdMeas, objType,...
148 |         objParameters);
149 |     
150 |     % Insert the measurements to the output matrix with relevant time stamp
151 |     surfaceMeasurements((i-1)*numMeasPerInstant + 1 : (i*numMeasPerInstant), :) = ...
152 |         [ones(numMeasPerInstant, 1)*iTime curMeasArray];
153 | end
154 | end
155 | 
156 | function [surfaceMeas] = produce_surface_measurements(pos, quat, numMeasurements, stdMeas, objType...
157 |     , objParameters)
158 | % PRODUCE_SURFACE_MEASUREMENTS generates 3D point measurements from the
159 | % surface of an object with specified geometry.
160 | switch objType
161 |     case 1 % Sphere
162 |         radius = objParameters;
163 |         
164 |         % Assume x,y and z are independent Gaussian random variables
165 |         sampleArray = randn(3, numMeasurements);
166 |         % Project each sample onto the sphere by normalizing the length
167 |         sampleArray = normc(sampleArray);
168 |         % Multiply the normalized sample to obtain desired radius
169 |         sampleArray = radius .* sampleArray;
170 |         
171 |         xTrue_L = sampleArray(1,:)';
172 |         yTrue_L = sampleArray(2,:)';
173 |         zTrue_L = sampleArray(3,:)';
174 |         
175 |     case 2 % Box
176 |         l = objParameters(1);
177 |         h = objParameters(2);
178 |         w = objParameters(3);
179 |         
180 |         % First, produce a random array for the selection of the faces
181 |         faceArray = randi([0 5], numMeasurements, 1);
182 |         
183 |         % Initialize the outputs
184 |         xTrue_L = zeros(numMeasurements, 1);
185 |         yTrue_L = zeros(numMeasurements, 1);
186 |         zTrue_L = zeros(numMeasurements, 1);
187 |         
188 |         % Select random points exactly on the specified box
189 |         pointer = 1;
190 |         for iFace = 0:5
191 |             num = sum(faceArray == iFace);
192 |             switch iFace
193 |                 case 0 % on X-Y plane, Z is positive
194 |                     xTrue_L(pointer:(pointer+num-1)) = -l/2 + l*rand(num, 1);
195 |                     yTrue_L(pointer:(pointer+num-1)) = -w/2 + w*rand(num, 1);
196 |                     zTrue_L(pointer:(pointer+num-1)) = h/2*ones(num,1);
197 |                 case 1 % on X-Y plane, Z is negative
198 |                     xTrue_L(pointer:(pointer+num-1)) = -l/2 + l*rand(num, 1);
199 |                     yTrue_L(pointer:(pointer+num-1)) = -w/2 + w*rand(num, 1);
200 |                     zTrue_L(pointer:(pointer+num-1)) = -h/2*ones(num,1);
201 |                 case 2 % on X-Z plane, Y is positive
202 |                     xTrue_L(pointer:(pointer+num-1)) = -l/2 + l*rand(num, 1);
203 |                     yTrue_L(pointer:(pointer+num-1)) = w/2*ones(num,1);
204 |                     zTrue_L(pointer:(pointer+num-1)) = -h/2 + h*rand(num, 1);
205 |                 case 3 % on X-Z plane, Y is negative
206 |                     xTrue_L(pointer:(pointer+num-1)) = -l/2 + l*rand(num, 1);
207 |                     yTrue_L(pointer:(pointer+num-1)) = -w/2*ones(num,1);
208 |                     zTrue_L(pointer:(pointer+num-1)) = -h/2 + h*rand(num, 1);
209 |                 case 4 % on Y-Z plane, X is positive
210 |                     xTrue_L(pointer:(pointer+num-1)) = l/2*ones(num,1);
211 |                     yTrue_L(pointer:(pointer+num-1)) = -w/2 + w*rand(num, 1);
212 |                     zTrue_L(pointer:(pointer+num-1)) = -h/2 + h*rand(num, 1);
213 |                 case 5 % on Y-Z plane, X is negative
214 |                     xTrue_L(pointer:(pointer+num-1)) = -l/2*ones(num,1);
215 |                     yTrue_L(pointer:(pointer+num-1)) = -w/2 + w*rand(num, 1);
216 |                     zTrue_L(pointer:(pointer+num-1)) = -h/2 + h*rand(num, 1);
217 |             end
218 |             pointer = pointer + num;
219 |         end   
220 | end
221 | 
222 | % Add Gaussian noise on selected points
223 | xNoisy_L = xTrue_L + stdMeas * randn(numMeasurements, 1);
224 | yNoisy_L = yTrue_L + stdMeas * randn(numMeasurements, 1);
225 | zNoisy_L = zTrue_L + stdMeas * randn(numMeasurements, 1);
226 | meas_L = [xNoisy_L'; yNoisy_L'; zNoisy_L'];
227 | 
228 | % Obtain the rotation matrix from local frame to the global
229 | R_from_G_to_L = rotation_matrix_from_global_to_local(quat);
230 | R_from_L_to_G = transpose(R_from_G_to_L);
231 | 
232 | % Rotate 3D points to the global frame
233 | meas_G = transpose(R_from_L_to_G * meas_L);
234 | 
235 | % Translate the measurements to the center of the object
236 | surfaceMeas = [meas_G(:,1)+pos(1) meas_G(:,2)+pos(2) meas_G(:,3)+pos(3)];
237 | end
238 | 
239 | function [ transformationMatrix ] = angular_velocity_transformation_matrix_from_local_to_global( quat )
240 | % The function computes the transformation matrix from Local (body) frame to the Global (inertial)
241 | % frame.
242 | % Note that this matrix is to multiply the angular velocity vector expressed in Local from from LEFT,
243 | % i.e., quatDot  =  transformationMatrix  *  wLocal
244 | 
245 | q0 = quat(1);
246 | q1 = quat(2);
247 | q2 = quat(3);
248 | q3 = quat(4);
249 | 
250 | % Rotation matrix from global to local R_from_G_to_L
251 | transformationMatrix = 1/2 * [-q1 -q2 -q3; q0 -q3 q2; q3 q0 -q1; -q2 q1 q0];
252 | end
253 | 


--------------------------------------------------------------------------------
/GPETT/rotation_matrix_from_global_to_local.m:
--------------------------------------------------------------------------------
 1 | function [ rotMatrix ] = rotation_matrix_from_global_to_local( quat )
 2 | % The function computes the rotation matrix from the global frame to the Local (body)
 3 | % frame.
 4 | % Note that the rotation matrix is to multiply the vector from LEFT,
 5 | % i.e., xL  =  rotMatrix  *  xG.
 6 | 
 7 | quat = quat(:);     % Makes sure the input is a column matrix
 8 | 
 9 | q = quat(1:3);
10 | q4 = quat(4);
11 | 
12 | % Rotation matrix from global to local R_from_G_to_L
13 | rotMatrix = transpose((q4^2-q'*q)*eye(3,3) + 2*(q*q') - 2*q4*skew_symmetric_matrix(q));
14 | end
15 | 
16 | function [out] = skew_symmetric_matrix(x)
17 | out = [0  -x(3)  x(2);...
18 |     x(3)  0  -x(1);...
19 |     -x(2)  x(1)  0];
20 | end
21 | 


--------------------------------------------------------------------------------
/GPETT/visualize_tracking_outputs.m:
--------------------------------------------------------------------------------
  1 | function [  ] = visualize_tracking_outputs( estState, estQuat, estStateVar, measArray...
  2 |     , groundTruth, time, basisAngleArray )
  3 | % VISUALIZE_TRACKING_OUTPUTS visualizes the 3D extent estimate with the
  4 | % associated uncertainty information. Besides, if the ground truth is
  5 | % available, it is plotted as well.
  6 | 
  7 | % Determine whether the ground truth information is provided
  8 | if isempty(groundTruth)
  9 |     isGTAvailable = 0;
 10 | else
 11 |     isGTAvailable = 1;
 12 | end
 13 | 
 14 | % Extract relevant information from the updated state
 15 | estCenter = estState(1:3);
 16 | % estVelocity = estState(4:6);
 17 | estExtent = estState(13:end);
 18 | 
 19 | % For debugging
 20 | % estExtent(estExtent<0) = 0;
 21 | 
 22 | % Extract extent variances
 23 | stdState = sqrt(estStateVar);
 24 | stdExtent = stdState(13:end);
 25 | 
 26 | % Extract ground truth values of the position and quaternions
 27 | if isGTAvailable
 28 |     objType = groundTruth.objectDescription(1);
 29 |     objParam = groundTruth.objectDescription(2:end);
 30 |     gtKinematics = groundTruth.dataLog(abs(groundTruth.dataLog(:,1)-time)<1e-10, 2:end);
 31 |     gtCenter =gtKinematics(1:3)';
 32 |     gtQuat = gtKinematics(4:7)';
 33 | end
 34 | 
 35 | cla(); % Clear the current axis
 36 | 
 37 | % Plot the point measurements
 38 | plot3(measArray(:,1), measArray(:,2), measArray(:,3), 'r+', 'LineWidth', 2, 'MarkerSize', 5);
 39 | 
 40 | % Plot the extent estimates with their uncertainty
 41 | plot_extent_with_uncertainty(estCenter, estQuat, estExtent, stdExtent, basisAngleArray);
 42 | 
 43 | % Plot the ground truth
 44 | if isGTAvailable
 45 |     switch objType
 46 |         case 1 % Sphere
 47 |             gTPlotHandle = plot_sphere(gtCenter, objParam, [1 0 1]);
 48 |         case 2 % Box
 49 |             gTPlotHandle = plot_box(gtCenter, gtQuat, objParam, [1 0 1]);
 50 |     end
 51 | end
 52 | 
 53 | % Attach a legend
 54 | legend('Measurements', 'Estimated surface', '1-sigma outer surface', 'Ground truth');
 55 | 
 56 | % Arrange lighting in the environment
 57 | delete(findall(gcf,'Type','light'))
 58 | lightangle(-60,60)
 59 | lighting phong;
 60 | material shiny;
 61 | drawnow();
 62 | end
 63 | 
 64 | function plotHandles =  plot_extent_with_uncertainty (objCenter, quaternion, extent, stdExtent, basisAngles)
 65 | 
 66 | % Calculate the rotation matrix from Local to Global by the quternions
 67 | R_from_G_to_L = rotation_matrix_from_global_to_local(quaternion);
 68 | R_from_L_to_G = transpose(R_from_G_to_L);
 69 | 
 70 | % Convert the extent into cartesion coordinates in Global frame
 71 | extentCart_G = transform_extent_from_local_to_global(extent, basisAngles...
 72 |     , objCenter, R_from_L_to_G);
 73 | 
 74 | % Extract x,y,z coordinates of extent estimates
 75 | xExtent = transpose(extentCart_G(1, :));
 76 | yExtent = transpose(extentCart_G(2, :));
 77 | zExtent = transpose(extentCart_G(3, :));
 78 | 
 79 | %% Plot the estimated surface
 80 | % First, construct the connectivity list considering the unit sphere
 81 | % evaluated at the basis angles
 82 | sphere = transform_extent_from_local_to_global(ones(length(basisAngles),1), basisAngles...
 83 |     , objCenter, R_from_L_to_G);
 84 | connectiviyList = boundary(sphere(1,:)', sphere(2,:)', sphere(3,:)', 0.2); % Perform triangulation
 85 | 
 86 | % Then, plot the estimated surface by triangulation
 87 | % sEst = trisurf(connectiviyList, xExtent, yExtent, zExtent, 'FaceAlpha', 0.6, 'FaceColor', [0.3 0.75 0.93]); % Plot the triangles
 88 | triangles = boundary(extentCart_G', 0.5);
 89 | sEst = trisurf(triangles, xExtent, yExtent, zExtent, 'FaceAlpha', 0.6, 'FaceColor', [0.3 0.75 0.93]); % Plot the triangles
 90 | sEst.EdgeColor = 'none';
 91 | 
 92 | %% Plot outer and inner surfaces regarding the uncertainty of estimates
 93 | extentInner = extent - sign(extent) .* min(abs(extent), stdExtent);
 94 | extentOuter = extent + sign(extent) .* stdExtent;
 95 | 
 96 | % Convert inner extent into cartesion coordinates in Global frame
 97 | innerCart_G = transform_extent_from_local_to_global(extentInner, basisAngles...
 98 |     , objCenter, R_from_L_to_G);
 99 | 
100 | % Convert outer extent into cartesion coordinates in Global frame
101 | outerCart_G =  transform_extent_from_local_to_global(extentOuter, basisAngles...
102 |     , objCenter, R_from_L_to_G);
103 | 
104 | % Plot the inner surface
105 | % sInner = trisurf(connectiviyList, innerCart_G(1,:)', innerCart_G(2,:)', innerCart_G(3,:)',...
106 | %     'FaceAlpha', 0.2, 'FaceColor', 'r'); % Plot the triangles
107 | % sInner.EdgeColor = 'none';
108 | 
109 | % Plot the outer surface
110 | triangles = boundary(outerCart_G', 0.5);
111 | sOuter = trisurf(triangles, outerCart_G(1,:)', outerCart_G(2,:)', outerCart_G(3,:)',...
112 |     'FaceAlpha', 0.1, 'FaceColor', [1 0.9 0]); % Plot the triangles
113 | sOuter.EdgeColor = 'none';
114 | 
115 | % Return the plot handles
116 | plotHandles = [sEst, sOuter];
117 | 
118 |     function  extentGlobal = transform_extent_from_local_to_global(extentLocal, basisAngles, objectCenter, rotMatrix)
119 |         
120 |         % Convert the extent into cartesion coordinates in Local frame
121 |         [x_L, y_L, z_L] = sph2cart(basisAngles(:,1), basisAngles(:,2), extentLocal);
122 |         cart_L = [x_L'; y_L'; z_L'];
123 |         
124 |         % Transform the extent to the Global frame
125 |         extentGlobal = objectCenter + rotMatrix * cart_L;
126 |     end
127 | end
128 | 
129 | function plotHandles = plot_box(origin, quaternions, objectParameters, color)
130 | % CUBE_PLOT plots a cube
131 | % INPUTS:
132 | %   origin = set origin point for the cube in the form of [x,y,z]
133 | %   edgeLength = length of an edge
134 | %   quaternions = [q0 q1 q2 q3] to determine orientation wrt global frame
135 | %   color  = STRING, the color patched for the cube.
136 | %         List of colors
137 | %         b blue
138 | %         g green
139 | %         r red
140 | %         c cyan
141 | %         m magenta
142 | %         y yellow
143 | %         k black
144 | %         w white
145 | 
146 | % Extract object parameters
147 | length = objectParameters(1);
148 | height = objectParameters(2);
149 | width = objectParameters(3);
150 | 
151 | % Define the vertices of the box
152 | vertices_L = [length width -height;
153 |     -length width -height;
154 |     -length width height;
155 |     length width height;
156 |     -length -width height;
157 |     length -width height;
158 |     length -width -height;
159 |     -length -width -height]* 0.5;
160 | 
161 | R_from_G_to_L = rotation_matrix_from_global_to_local(quaternions);
162 | R_from_L_to_G = transpose(R_from_G_to_L);
163 | vertices_G = transpose(R_from_L_to_G * vertices_L');
164 | 
165 | %  Define the faces of the box
166 | fac = [1 2 3 4;
167 |     4 3 5 6;
168 |     6 7 8 5;
169 |     1 2 8 7;
170 |     6 7 1 4;
171 |     2 3 5 8];
172 | box = [vertices_G(:,1)+origin(1),vertices_G(:,2)+origin(2),vertices_G(:,3)+origin(3)];
173 | % plotHandles = patch('Faces',fac,'Vertices',box,'FaceColor',color, 'EdgeColor'...
174 | %     , 'none', 'LineWidth', 1,'LineStyle', ':','FaceAlpha', 0.35);
175 | 
176 | plotHandles = patch('Faces',fac,'Vertices',box,'FaceColor',color, 'EdgeColor'...
177 |     , [0.3 0.3 0.3], 'LineWidth', 1,'FaceAlpha', 0);
178 | end
179 | 
180 | function plotHandles = plot_sphere(center, objectParameters, color)
181 | % Plots a sphere
182 | 
183 | % Extract object parameters
184 | radius = objectParameters;% Ellipsoid
185 | 
186 | % Produce cartesian points on the unit sphere
187 | [xSphere, ySphere, zSphere] = sphere(128);
188 | 
189 | % Scale and translate these points
190 | xSphere = xSphere*radius + center(1);
191 | ySphere = ySphere*radius + center(2);
192 | zSphere = zSphere*radius + center(3);
193 | 
194 | % Plot the surface
195 | h = surfl(xSphere, ySphere, zSphere);
196 | h.FaceColor = color;
197 | h.FaceAlpha = 0.3;
198 | h.EdgeColor = 'none';
199 | 
200 | % Return the surface handle
201 | plotHandles = h;
202 | end
203 | 
204 | 
205 | 
206 | 


--------------------------------------------------------------------------------
/GPETTP/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 | % Output:
 4 | %           covMatrix:      The covariance matrix computed by the GP kernel.
 5 | % Inputs:
 6 | %           argArray1:       Each row is a polar angle.
 7 | %           argArray2:       Each row is a polar angle.
 8 | %           paramGP:         Parameters of the underlying GP
 9 | 
10 | % Extract parameters
11 | kernelType = paramGP{1};
12 | stdPrior = paramGP{2};
13 | stdRadius  = paramGP{3};
14 | scaleLength = paramGP{4};
15 | isObjectSymmetric = paramGP{5};
16 | 
17 | switch kernelType
18 |     case 1
19 |         % Periodic squared exponential kernel
20 |         diffMatrix = compute_diffence_matrix(argArray1, argArray2);
21 |         
22 |         if isObjectSymmetric
23 |             % Covariance periodic with pi (it imposes the symmetry assumpation of the object)
24 |             covMatrix = stdPrior^2 * exp(-sin(diffMatrix).^2 / (2*scaleLength^2)) + stdRadius^2;
25 |         else
26 |             % Covariance periodic with 2*pi
27 |             covMatrix = stdPrior^2 * exp(-2 * sin(diffMatrix/2).^2 / scaleLength^2) + stdRadius^2;
28 |         end                
29 | end
30 | end
31 | 
32 | function [ diffMatrix ] = compute_diffence_matrix( inp1, inp2 )
33 | % This function calculates the angle matrix regarding two inputs.
34 | 
35 | % Produce a grid structure to be able to compute each angle between
36 | % elements of two inputs.
37 | len1 = size(inp1, 1); % The number of polar angles in the first input
38 | len2 = size(inp2, 1); % The number of polar angles in the second input
39 | 
40 | % Create matrices from these vectors to form grid structure
41 | grid1 = repmat(inp1, [1, len2]);
42 | grid2 = repmat(transpose(inp2), [len1, 1]);
43 | 
44 | % Compute the difference matrix to be used for covariance matrix calculation
45 | diffMatrix = grid1-grid2;
46 | end
47 | 


--------------------------------------------------------------------------------
/GPETTP/demo.m:
--------------------------------------------------------------------------------
  1 | % This file presents a demonstration of the extended tracker proposed in 
  2 | % Section VII of the following study.
  3 | 
  4 | % For the details, please see:
  5 | %       M. Kumru and E. Özkan, “Three-Dimensional Extended Object Tracking 
  6 | %       and Shape Learning Using Gaussian Processes”, 
  7 | %       IEEE Transactions on Aerospace and Electronic Systems, 2021.
  8 | %       https://ieeexplore.ieee.org/abstract/document/9382878/
  9 | 
 10 | clc;
 11 | clear;
 12 | close all;
 13 | 
 14 | 
 15 | %% Paramaters
 16 | % Simulation Parameters
 17 | expDuration = 30;   % (in seconds)
 18 | T = 0.1;            % sampling time (in seconds)
 19 | eps = 1e-6;         % a small scalar
 20 | 
 21 | % Measurement Parameters
 22 | numMeasPerInstant = 20;
 23 | paramMeas = {numMeasPerInstant, expDuration, T};
 24 | 
 25 | % Gaussian Process (GP) Parameters
 26 | numBasisAngles = 50;            % number of basis angles on which extent of each projection is maintained
 27 | meanGP = 0;                     % mean of the GP
 28 | stdPriorGP = 1;                 % prior standard deviation of the GP
 29 | stdRadiusGP = 0.2;              % standard deviation of the radius
 30 | scaleLengthGP = pi/5;           % scale length
 31 | kernelTypeGP = 1;               % 1:Squared exponential, 2: Matern3_2, 3: Matern5_2
 32 | isObjSymmetric = 0;             % it is used to adjust the kernel function accordingly
 33 | stdMeasGP = 0.2;                % standard deviation of the measurements (used in the GP model)
 34 | paramGP1 = {kernelTypeGP, stdPriorGP, stdRadiusGP, scaleLengthGP, 0, stdMeasGP, meanGP};
 35 | paramGP2 = {kernelTypeGP, stdPriorGP, stdRadiusGP, scaleLengthGP, 0, stdMeasGP, meanGP};
 36 | paramGP3 = {kernelTypeGP, stdPriorGP, stdRadiusGP, scaleLengthGP, 0, stdMeasGP, meanGP};
 37 | paramGP = {paramGP1, paramGP2, paramGP3};
 38 | 
 39 | % EKF Paramaters
 40 | stdCenter = 1e-1;               % std dev of the object center
 41 | stdAngVel = 4e-1;               % std dev of each Euler angle
 42 | lambda = 0.99;
 43 | 
 44 | % Parameters of the Scaling
 45 | meanS = 5/6;                    % mean of the scaling parameter
 46 | stdS = sqrt(1/18);              % standard deviation of the scaling parameter
 47 | paramScaling = [meanS stdS];
 48 | 
 49 | % Determine the Basis Angles for 2D Contour
 50 | basisAngleArray = transpose(linspace(0, 2*pi, numBasisAngles+1));
 51 | basisAngleArray(end) = [];      % Trash the end point to eleminate repetition at 2*pi
 52 | 
 53 | 
 54 | %% Load Measurements and Ground Truth
 55 | [measComplete, groundTruth] = get_measurements(paramMeas);
 56 | 
 57 | 
 58 | %% Determine the Initial Distribution
 59 | % Initial covariance
 60 | P0_c = 0.1 * eye(3);
 61 | P0_v = 0.1 * eye(3);
 62 | P0_a = 1e-4 * eye(3,3);
 63 | P0_w = 1e-4 * eye(3,3);
 64 | P0_extent1 = compute_GP_covariance(basisAngleArray, basisAngleArray, paramGP{1});
 65 | P0_extent2 = compute_GP_covariance(basisAngleArray, basisAngleArray, paramGP{2});
 66 | P0_extent3 = compute_GP_covariance(basisAngleArray, basisAngleArray, paramGP{3});
 67 | P0 = blkdiag(P0_c, P0_v,  P0_a, P0_w, P0_extent1, P0_extent2, P0_extent3);
 68 | 
 69 | % Initial mean
 70 | c0 = groundTruth.dataLog(1, 2:4)' + sqrt(0.1)*randn(3,1);       % initial position
 71 | v0 = [0.5 0 0]' + sqrt(0.1)*randn(3,1);                         % initial linear velocity
 72 | q0 = [0 0 0 1]';                                                % initial quaternion
 73 | a0 = [0 0 0]';                                                  % initial orientation deviation vector
 74 | w0 = [0 0 0]';                                                  % initial angular  velocity
 75 | f1_0 = meanGP * ones(numBasisAngles, 1);                        % initial extent for the first projection (is initialized regarding the GP model)
 76 | f2_0 = f1_0;                                                    % initial extent for the second projection
 77 | f3_0 = f1_0;                                                    % initial extent for the third projection
 78 | f0 = [f1_0; f2_0; f3_0];                                        % initial value of the state vector
 79 | 
 80 | % Initialize the filter estimate
 81 | estState = [c0; v0; a0; w0; f0];
 82 | estQuat = q0;
 83 | estStateCov = P0;
 84 | 
 85 | 
 86 | %% Define the Process Model
 87 | F_lin = kron([1 T; 0 1], eye(3,3));         % constant velocity model for linear motion
 88 | F_rot = eye(6,6);                           % a substitute matrix for the rotational dynamics (it will be computed in the filter at each recursion)
 89 | F_f1 = eye(numBasisAngles);      
 90 | F_f2 = F_f1;
 91 | F_f3 = F_f1;
 92 | F_f = blkdiag(F_f1, F_f2, F_f3);
 93 | F = blkdiag(F_lin, F_rot, F_f);
 94 | 
 95 | Q_lin = kron([T^3/3 T^2/2; T^2/2 T], stdCenter^2*eye(3));    % process noise covariance of linear motion
 96 | Q_rot = eye(6,6);                                            % a substitute matrix for the covariance of the rotational motion (it will be computed in the filter at each recursion)
 97 | Q_extent1 = zeros(numBasisAngles);                       
 98 | Q_extent2 = Q_extent1;
 99 | Q_extent3 = Q_extent1;
100 | Q_extent = blkdiag(Q_extent1, Q_extent2, Q_extent3);
101 | Q = blkdiag(Q_lin, Q_rot, Q_extent);
102 | 
103 | processModel = {F, Q, lambda};
104 | 
105 | 
106 | %% Open a Figure (to illustrate the tracking outputs)
107 | figure;
108 | grid on; hold on; axis equal;
109 | view(45, 25);
110 | title('Extended Target Tracking Results');
111 | xlabel('X axis');
112 | ylabel('Y axis');
113 | zlabel('Z axis');
114 | 
115 | 
116 | %% GPETT3DProjection LOOP
117 | time = 0;
118 | numInstants = ceil(expDuration/ T);
119 | filterParam = {processModel, paramGP, paramScaling, basisAngleArray, T, stdAngVel, eps}; 
120 | 
121 | for k = 1:numInstants
122 |     
123 |     % Extract current measurements
124 |     curMeasArray = measComplete(abs(measComplete(:, 1)-time)<eps, 2:4);
125 |     
126 |     % Call the GPETT3DProjection filter
127 |     [estState, estQuat, estStateCov] = filter_GPETT3DProjection(estState, estQuat...
128 |         , estStateCov, curMeasArray, filterParam);
129 |     
130 |     % Visualize the results
131 |     visualize_tracking_outputs(estState, estQuat, diag(estStateCov), curMeasArray, groundTruth...
132 |         , time, basisAngleArray);
133 |     
134 |     % Update time
135 |     time = time + T;
136 | end
137 | 


--------------------------------------------------------------------------------
/GPETTP/filter_GPETT3DProjection.m:
--------------------------------------------------------------------------------
  1 | function [ estState, estQuat, estStateCov ] = filter_GPETT3DProjection( prevEstState...
  2 |     , prevEstQuat, prevEstStateCov, measArray, filterParam )
  3 | 
  4 | % Extract the parameters 
  5 | processModel = filterParam{1};
  6 | paramGP = filterParam{2};
  7 | paramScaling = filterParam{3};
  8 | basisAngleArray = filterParam{4};
  9 | T = filterParam{5};
 10 | stdAngVel = filterParam{6};
 11 | eps = filterParam{7};
 12 | 
 13 | F = processModel{1};
 14 | Q = processModel{2};
 15 | lambda = processModel{3};
 16 | 
 17 | % Extract scaling parameters
 18 | meanS = paramScaling(1);
 19 | stdS = paramScaling(2);
 20 | 
 21 | stdMeasGP = paramGP{1}{6};
 22 | 
 23 | % Compute the inverse of P0_extent since it will repeatedly be used in the system dynamic model
 24 | persistent inv_P0_extent1 inv_P0_extent2 inv_P0_extent3;
 25 | if isempty(inv_P0_extent1)
 26 |     inv_P0_extent1 = compute_inverse_GP_covariance(basisAngleArray, paramGP{1}, eps);
 27 |     inv_P0_extent2 = compute_inverse_GP_covariance(basisAngleArray, paramGP{2}, eps);
 28 |     inv_P0_extent3 = compute_inverse_GP_covariance(basisAngleArray, paramGP{3}, eps);
 29 | end
 30 | 
 31 | % Defining the Projection Matrices
 32 | % i.e., let X be a vector in R^3, x_p1 = P1*X is the projection of X on Plane-1
 33 | persistent P1 P2 P3;
 34 | if isempty(P1)
 35 |     P1 = [1 0 0; 0 1 0];    % projects to Plane-1 (X-Y plane)
 36 |     P2 = [1 0 0; 0 0 1];    % projects to Plane-2 (X-Z plane)
 37 |     P3 = [0 1 0; 0 0 1];    % projects to Plane-3 (Y-Z plane)
 38 | end
 39 | 
 40 | 
 41 | %% Process Update
 42 | % Compute the rotational dynamic model
 43 | angVelEst = prevEstState(10:12);
 44 | [FRot, QRot] = compute_rotational_dynamics(angVelEst, stdAngVel, T);
 45 | 
 46 | % Substitute the rotational model
 47 | F(7:12, 7:12) = FRot;
 48 | Q(7:12, 7:12) = QRot;
 49 | 
 50 | % Compute predicted state and covariance
 51 | predState = F * prevEstState;
 52 | predQuatDev = predState(7:9);
 53 | predStateCov = F * prevEstStateCov * F' + Q;
 54 | 
 55 | % Dynamic model for maximum entropy in extent
 56 | predStateCov(13:end, 13:end) = prevEstStateCov(13:end, 13:end)/ lambda;
 57 | 
 58 | % Extract relevant information from the predicted state
 59 | predCenter = predState(1:3);
 60 | predVelocity = predState(4:6);
 61 | numBasisAngles = size(basisAngleArray,1);
 62 | predExtent_P1 = predState(13:12+numBasisAngles);
 63 | predExtent_P2 = predState(13+numBasisAngles:12+2*numBasisAngles);
 64 | predExtent_P3 = predState(13+2*numBasisAngles:12+3*numBasisAngles);
 65 | 
 66 | 
 67 | %% Measurement Update
 68 | curNumMeas = size(measArray, 1);
 69 | numStates = size(F,1);
 70 | 
 71 | % In the below loops, three operations are performed:
 72 | % 1. Compute measurement predictions of the predicted state by the original
 73 | % nonlinear measurement model.
 74 | % 2. Obtain the corresponding measurement covariance matrix.
 75 | % 3. Linearize the measurement model to employ in EKF equations.
 76 | predMeas_P1 = zeros(curNumMeas * 2, 1);             % of the form [x1; y1;...; xn; yn]
 77 | predMeas_P2 = zeros(curNumMeas * 2, 1);
 78 | predMeas_P3 = zeros(curNumMeas * 2, 1);
 79 | measCov_P1 = zeros(curNumMeas * 2);                 % covariance matrix of projection 1
 80 | measCov_P2 = zeros(curNumMeas * 2);
 81 | measCov_P3 = zeros(curNumMeas * 2);
 82 | dH_dX_P1 = zeros(curNumMeas * 2, numStates);        % partial derivatives wrt states
 83 | dH_dX_P2 = zeros(curNumMeas * 2, numStates);
 84 | dH_dX_P3 = zeros(curNumMeas * 2, numStates);
 85 | dH_dY_P1 = zeros(curNumMeas * 2, 3);                % partial derivatives wrt 3D measurement
 86 | dH_dY_P2 = zeros(curNumMeas * 2, 3);                % partial derivatives wrt 3D measurement
 87 | dH_dY_P3 = zeros(curNumMeas * 2, 3);                % partial derivatives wrt 3D measurement
 88 | 
 89 | % Compute the rotation matrix. It will be used to transform the
 90 | % measurements into the local frame
 91 | R_from_G_to_L = rotation_matrix_from_global_to_local(apply_quat_deviation(predQuatDev, prevEstQuat));
 92 | 
 93 | % Plane 1
 94 | for i = 1:curNumMeas
 95 |     iMeas3D_G = transpose(measArray(i, :));
 96 |     
 97 |     % Compute the following variables to exploit in measurement model
 98 |     [p, H_f, covMeasBasis, angleMeas_L] = compute_measurement_model_components...
 99 |         (iMeas3D_G, predState, prevEstQuat, P1, paramGP{1}...
100 |         , basisAngleArray, inv_P0_extent1);
101 |     H_tilda = p * H_f;
102 |     
103 |     % Obtain the prediction for the nonlinear implicit measurement
104 |     % model, h(x,y) = 0
105 |     iMeasModelPredicted = P1 * R_from_G_to_L * (iMeas3D_G - predCenter)...
106 |         - meanS * H_tilda * predExtent_P1;
107 |     predMeas_P1(1+(i-1)*2 : i*2) = iMeasModelPredicted;
108 |     
109 |     % Obtain the covariance of the current measurement
110 |     % Definition: iCovMeas = k(uk,uk), uk: argument of the current measurement
111 |     iCovMeas = compute_GP_covariance(angleMeas_L, angleMeas_L, paramGP{1});
112 |     % Definition: iR_f = k(uk,uk) - k(uk, uf) * inv(K(uf,uf)) * k(uf, uk)
113 |     iR_f = iCovMeas - covMeasBasis * inv_P0_extent1 * covMeasBasis';
114 |     % Compute the total covariance of this specific measurement
115 |     iR =  stdMeasGP^2 * eye(2) + stdS^2*p*iR_f*p' + stdS^2*H_tilda...
116 |         *(predExtent_P1*predExtent_P1')* H_tilda';
117 |     % Plug this matrix into the overall covariance matrix
118 |     measCov_P1(((i-1)*2+1):i*2, ((i-1)*2+1):i*2) = iR;
119 |     
120 |     %% Obtain the partial derivatives of the measurement model
121 |     [H_center, H_velocity, H_quatDev, H_angVel, H_extent, H_meas] = compute_linearized_measurement_matrix...
122 |         (iMeas3D_G, predState, prevEstQuat, P1, predExtent_P1...
123 |         , paramGP{1}, basisAngleArray, inv_P0_extent1, meanS, eps);
124 |     
125 |     dH_dX_P1(1+(i-1)*2:i*2, :) = [H_center H_velocity H_quatDev H_angVel...
126 |         H_extent zeros(2, numBasisAngles)  zeros(2, numBasisAngles)];
127 |     
128 |     dH_dY_P1(1+(i-1)*2:i*2, :) = H_meas;
129 | end
130 | 
131 | % Plane 2
132 | for i = 1:curNumMeas
133 |     iMeas3D_G = transpose(measArray(i, :));
134 |     
135 |     % Compute the following variables to exploit in measurement model
136 |     [p, H_f, covMeasBasis, angleMeas_L] = compute_measurement_model_components...
137 |         (iMeas3D_G, predState, prevEstQuat, P2, paramGP{2}...
138 |         , basisAngleArray, inv_P0_extent2);
139 |     H_tilda = p * H_f;
140 |     
141 |     % Obtain the prediction for the nonlinear implicit measurement
142 |     % model, h(x,y) = 0
143 |     iMeasModelPredicted = P2 * R_from_G_to_L * (iMeas3D_G - predCenter)...
144 |         - meanS * H_tilda * predExtent_P2;
145 |     predMeas_P2(1+(i-1)*2 : i*2) = iMeasModelPredicted;
146 |     
147 |     % Obtain the covariance of the current measurement
148 |     % Definition: iCovMeas = k(uk,uk), uk: argument of the current measurement
149 |     iCovMeas = compute_GP_covariance(angleMeas_L, angleMeas_L, paramGP{2});
150 |     % Definition: iR_f = k(uk,uk) - k(uk, uf) * inv(K(uf,uf)) * k(uf, uk)
151 |     iR_f = iCovMeas - covMeasBasis * inv_P0_extent2 * covMeasBasis';
152 |     % Compute the total covariance of this specific measurement
153 |     iR =  stdMeasGP^2 * eye(2) + stdS^2*p*iR_f*p' + stdS^2*H_tilda...
154 |         *(predExtent_P2*predExtent_P2')* H_tilda';
155 |     % Plug this matrix into the overall covariance matrix
156 |     measCov_P2(((i-1)*2+1):i*2, ((i-1)*2+1):i*2) = iR;
157 |     
158 |     %% Obtain the partial derivatives of the measurement model
159 |     [H_center, H_velocity, H_quatDev, H_angVel, H_extent, H_meas] = compute_linearized_measurement_matrix...
160 |         (iMeas3D_G, predState, prevEstQuat, P2, predExtent_P2...
161 |         , paramGP{2}, basisAngleArray, inv_P0_extent2, meanS, eps);
162 |     
163 |     dH_dX_P2(1+(i-1)*2:i*2, :) = [H_center H_velocity H_quatDev H_angVel...
164 |         zeros(2, numBasisAngles) H_extent zeros(2, numBasisAngles)];
165 |     
166 |     dH_dY_P2(1+(i-1)*2:i*2, :) = H_meas;
167 | end
168 | 
169 | % Plane 3
170 | for i = 1:curNumMeas
171 |     iMeas3D_G = transpose(measArray(i, :));
172 |     
173 |     % Compute the following variables to exploit in measurement model
174 |     [p, H_f, covMeasBasis, angleMeas_L] = compute_measurement_model_components...
175 |         (iMeas3D_G, predState, prevEstQuat, P3, paramGP{3}...
176 |         , basisAngleArray, inv_P0_extent3);
177 |     H_tilda = p * H_f;
178 |     
179 |     % Obtain the prediction for the nonlinear implicit measurement
180 |     % model, h(x,y) = 0
181 |     iMeasModelPredicted = P3 * R_from_G_to_L * (iMeas3D_G - predCenter)...
182 |         - meanS * H_tilda * predExtent_P3;
183 |     predMeas_P3(1+(i-1)*2 : i*2) = iMeasModelPredicted;
184 |     
185 |     % Obtain the covariance of the current measurement
186 |     % Definition: iCovMeas = k(uk,uk), uk: argument of the current measurement
187 |     iCovMeas = compute_GP_covariance(angleMeas_L, angleMeas_L, paramGP{3});
188 |     % Definition: iR_f = k(uk,uk) - k(uk, uf) * inv(K(uf,uf)) * k(uf, uk)
189 |     iR_f = iCovMeas - covMeasBasis * inv_P0_extent3 * covMeasBasis';
190 |     iR = stdMeasGP^2 * eye(2) + stdS^2*p*iR_f*p' + stdS^2*H_tilda...
191 |         *(predExtent_P3*predExtent_P3')* H_tilda';
192 |     % Plug this matrix into the overall covariance matrix
193 |     measCov_P3(((i-1)*2+1):i*2, ((i-1)*2+1):i*2) = iR;
194 |     
195 |     %% Obtain the partial derivatives of the measurement model
196 |     [H_center, H_velocity, H_quatDev, H_angVel, H_extent, H_meas] = compute_linearized_measurement_matrix...
197 |         (iMeas3D_G, predState, prevEstQuat, P3, predExtent_P3...
198 |         , paramGP{3}, basisAngleArray, inv_P0_extent3, meanS, eps);
199 |     
200 |     dH_dX_P3(1+(i-1)*2:i*2, :) = [H_center H_velocity H_quatDev H_angVel...
201 |         zeros(2, numBasisAngles) zeros(2, numBasisAngles) H_extent];
202 |     
203 |     dH_dY_P3(1+(i-1)*2:i*2, :) = H_meas;
204 | end
205 | 
206 | 
207 | % Put the measurements in a column the form: [x1; y1;...; xn; yn]
208 | predMeas =  [predMeas_P1; predMeas_P2; predMeas_P3];
209 | % Measurement Update
210 | dH_dX = [dH_dX_P1; dH_dX_P2; dH_dX_P3];
211 | dH_dY = [dH_dY_P1; dH_dY_P2; dH_dY_P3];
212 | measCov = blkdiag(measCov_P1, measCov_P2, measCov_P3);
213 | kalmanGain = predStateCov * dH_dX' / (dH_dX*predStateCov*dH_dX' + measCov);
214 | estState = predState + kalmanGain * (0 - predMeas);
215 | estStateCov = (eye(numStates) - kalmanGain*dH_dX) * predStateCov;
216 | estStateCov = (estStateCov + estStateCov')/2; % To make it symmetric (eliminating numeric errors)
217 | 
218 | % Quaternion Treatment
219 | estQuatDev = estState(7:9);
220 | estQuat = apply_quat_deviation(estQuatDev, prevEstQuat);
221 | estState(7:9) = zeros(3,1);     % Reset the orientation deviation
222 | end
223 | 
224 | function [ H_center, H_velocity, H_quatDev, H_angVel, H_extent, H_meas] = compute_linearized_measurement_matrix...
225 |     (meas3D_G, state, quatPrev, projectionMatrix, extent, kernelParameters, basisAngleArray, inv_P0_extent, meanS, eps)
226 | 
227 | center = state(1:3);
228 | quatDev = state(7:9);
229 | quat = apply_quat_deviation(quatDev, quatPrev);
230 | 
231 | R_from_G_to_L = rotation_matrix_from_global_to_local(quat);
232 | 
233 | %% Obtain the measurement prediction by the original nonlinear implicit meas model
234 | [p, H_f, ~, ~] = compute_measurement_model_components(meas3D_G, state, quatPrev...
235 |     , projectionMatrix, kernelParameters, basisAngleArray, inv_P0_extent);
236 | measModelOriginal = projectionMatrix * R_from_G_to_L * (meas3D_G - center)...
237 |     - meanS * p * H_f * extent;
238 | 
239 | %% Calculate partial derivative wrt target extent
240 | H_extent = - meanS * p * H_f;     % This partial derivative is taken analytically.
241 | 
242 | %% Calculate partial derivative wrt linear positions
243 | H_center = zeros(2,3);
244 | for i = 1:3
245 |     stateIncremented = state;   % Initialize the vector
246 |     stateIncremented(i) = stateIncremented(i) + eps;    % increment the state
247 |     centerIncremented = stateIncremented(1:3);          % incremented center
248 |     
249 |     % Compute the output of original measurement model for the incremented state vector
250 |     [p, H_f, ~, ~] = compute_measurement_model_components(meas3D_G, stateIncremented, quatPrev...
251 |         , projectionMatrix, kernelParameters, basisAngleArray, inv_P0_extent);
252 |     measModelForIncState= projectionMatrix * R_from_G_to_L * (meas3D_G - centerIncremented)...
253 |         - meanS * p * H_f * extent;
254 |     
255 |     % Compute numeric derivative
256 |     H_center(:, i) = (measModelForIncState - measModelOriginal) / eps;
257 | end
258 | 
259 | %% Calculate partial derivative wrt linear velocities
260 | H_velocity = zeros(2,3); % Measurement model does not depend on linear velocities
261 | 
262 | %% Calculate partial derivative wrt quaternion deviations
263 | H_quatDev = zeros(2,3);
264 | for i = 7:9
265 |     stateIncremented = state;   % Initialize the vector
266 |     stateIncremented(i) = stateIncremented(i) + eps;
267 |     quatIncremented = apply_quat_deviation(stateIncremented(7:9), quatPrev);
268 |     
269 |     % Compute the output of original measurement model for the incremented state vector
270 |     [p, H_f, ~, ~] = compute_measurement_model_components(meas3D_G, stateIncremented, quatPrev...
271 |         , projectionMatrix, kernelParameters, basisAngleArray, inv_P0_extent);
272 |     
273 |     % Compute the rotation matrix for the incremented quaternion
274 |     R_from_G_to_L_Incremented = rotation_matrix_from_global_to_local(quatIncremented);
275 |     
276 |     % Compute the output of original measurement model for the incremented state vector
277 |     measModelForIncState = projectionMatrix * R_from_G_to_L_Incremented * (meas3D_G - center)...
278 |         - meanS * p * H_f * extent;
279 |     
280 |     % Compute numeric derivative
281 |     H_quatDev(:,i-6) = (measModelForIncState - measModelOriginal) / eps;
282 | end
283 | 
284 | %% Calculate partial derivative wrt angular velocities
285 | H_angVel = zeros(2,3); % Measurement model does not depend on linear velocities
286 | 
287 | %% Calculate partial derivatives wrt measurement
288 | H_meas = projectionMatrix * R_from_G_to_L;
289 | 
290 | end
291 | 
292 | function [diffUnitVector, H_f, covMeasBasis, angle] = compute_measurement_model_components...
293 |     (meas3D_G, state, quatPrev, projectionMatrix, paramGP, basisAngleArray, inv_P0_extent)
294 | 
295 | center = state(1:3);
296 | quatDev = state(7:9);
297 | quat = apply_quat_deviation(quatDev, quatPrev); % Apply predicted orientation deviation
298 | 
299 | % Compute the following variables to exploit in measurement model
300 | R_from_G_to_L = rotation_matrix_from_global_to_local(quat);
301 | 
302 | % Transform the measurements to the local frame
303 | meas3D_L = R_from_G_to_L * (meas3D_G - center);
304 | 
305 | % Project the local measurement onto the corresponding projection plane
306 | measProjected = projectionMatrix * meas3D_L;
307 | 
308 | diffVector = measProjected;
309 | diffVectorMag = norm(diffVector);
310 | if diffVectorMag == 0
311 |     diffVectorMag = eps;
312 | end
313 | diffUnitVector = diffVector / diffVectorMag;
314 | 
315 | angle = atan2(diffVector(2), diffVector(1));  % angle of the projected measurement on the projection plane
316 | angle = mod(angle, 2*pi);
317 | 
318 | covMeasBasis = compute_GP_covariance(angle, basisAngleArray, paramGP);
319 | 
320 | H_f = covMeasBasis * inv_P0_extent; % GP model relating the extent and the radial
321 | end
322 | 
323 | function [inv_P0_extent] = compute_inverse_GP_covariance(basisAngleArray, paramGP, eps)
324 | P0_extent = compute_GP_covariance(basisAngleArray, basisAngleArray, paramGP);
325 | P0_extent = P0_extent + eps*eye(size(basisAngleArray,1));       % to prevent numerical errors thrown in matrix inversion
326 | chol_P0_extent = chol(P0_extent);
327 | inv_chol_P0_extent = inv(chol_P0_extent);
328 | inv_P0_extent = inv_chol_P0_extent * inv_chol_P0_extent';
329 | end
330 | 
331 | function [F, Q] = compute_rotational_dynamics(w, std, T)
332 | % Inputs:
333 | %              w:       Angular velocity
334 | %              std:     Standard deviation of the angular velocity
335 | %              T:       Sampling time
336 | 
337 | % Dummy variables
338 | wNorm = norm(w);
339 | if wNorm == 0
340 |     wNorm = 1e-3;
341 | end
342 | 
343 | S = skew_symmetric_matrix(-w);
344 | c = cos(1/2*T*wNorm);
345 | s = sin(1/2*T*wNorm);
346 | expMat = eye(3,3) + s/wNorm*S + (1-c)/wNorm^2*S^2;
347 | 
348 | % Construct the state transition matrix
349 | F = [expMat  T*expMat ; zeros(3,3)  eye(3,3)];
350 | 
351 | % Construct the process noise covariance matrix
352 | G11 = T*eye(3,3) + 2/wNorm^2*(1-c)*S + 1/wNorm^2*(T-2/wNorm*s)*S^2;
353 | G12 = 1/2*T^2*eye(3,3) + 1/wNorm^2*(4/wNorm*s-2*T*c)*S + 1/wNorm^2*(1/2*T^2+2/wNorm*T*s+4/wNorm^2*(c-1))*S^2;
354 | G21 = zeros(3,3);
355 | G22 = T * eye(3,3);
356 | G = [G11 G12; G21 G22];
357 | 
358 | B = G * [zeros(3,3); eye(3,3)];
359 | 
360 | % cov = std^2 * diag([0 0 1]);
361 | cov = std^2 * eye(3,3);
362 | Q = B * cov * B';
363 | end
364 | 
365 | function [out] = skew_symmetric_matrix(x)
366 | out = [0  -x(3)  x(2);...
367 |     x(3)  0  -x(1);...
368 |     -x(2)  x(1)  0];
369 | end
370 | 
371 | function [qOut] = apply_quat_deviation(a, qIn)
372 | qa = 1/ sqrt(4+norm(a)) * [a; 2];   % It depends on the Rodrigues parametrization
373 | qOut = quat_product(qa, qIn);
374 | 
375 | qOut = qOut/ norm(qOut);    % Normalize due to numeric inprecisions
376 | end
377 | 
378 | function [out] = quat_product(q1, q2)
379 | q1V = q1(1:3);
380 | q1S = q1(4);
381 | q2V = q2(1:3);
382 | q2S = q2(4);
383 | 
384 | out = [q1S*q2V + q2S*q1V - cross(q1V,q2V);...
385 |     q1S*q2S - q1V'*q2V];
386 | end


--------------------------------------------------------------------------------
/GPETTP/get_measurements.m:
--------------------------------------------------------------------------------
  1 | function [ measComplete, groundTruth ] = get_measurements( paramMeas )
  2 | %GET_MEASUREMENTS provides point measurements. 
  3 | % Outputs:
  4 | %	MeasComplete: is the complete set of point measurements. Each row is in
  5 | %       the form of [timeStamp x y z]
  6 | %   groundTruth: is the ground truth information of the corresponding scenario.
  7 | %       It is a structure with the following fields: 'objectDescription', 'dataRecord'.
  8 | %       'objectDescription' includes type (e.g., sphere, box...) and the parameters
  9 | %       of the geometry.
 10 | %       Each row of the 'dataRecord' is in the form of [timeStamp center' quaternions']
 11 | %       where centerPosition = [cX cY cZ]' and quaternions = [q0 q1 q2 q3]'.
 12 | 
 13 | % Input:
 14 | %   paramMeas: measurement parameters. It is a cell variable including the
 15 | %       following information {numMeasPerInstant, expDuration, T}
 16 | 
 17 | % Extract the parameters
 18 | numMeasPerInstant = paramMeas{1};
 19 | expDuration = paramMeas{2};
 20 | T = paramMeas{3};
 21 | numInstants = ceil(expDuration/ T);
 22 | stdMeas = 0.1;              % Measurement noise std
 23 | 
 24 | % Object parameters
 25 | objType = 2; % objType = 1:Sphere, 2:Box
 26 | switch objType
 27 |     case 1 % Sphere
 28 |         radius = 2;
 29 |         objParameters = radius;
 30 |     case 2 % Box
 31 |         l = 3;      % Length
 32 |         h = 3;      % Height
 33 |         w = 3;      % Width
 34 |         objParameters = [l h w];
 35 | end
 36 | 
 37 | % Generate the measurements in Matlab
 38 | [measComplete, gtDataLog] = generate_measurements_Matlab...
 39 |     (stdMeas, numMeasPerInstant, objType, objParameters, numInstants, T);
 40 | 
 41 | groundTruth = struct('objectDescription', [objType, objParameters]...
 42 |     , 'dataLog', gtDataLog);
 43 | 
 44 | end
 45 | 
 46 | function [surfaceMeasurements, groundTruth] = generate_measurements_Matlab...
 47 |     (stdMeas, numMeasPerInstant, objType, objParameters, numInstants, T)
 48 | % This function produces surface measurements from a 3D object.
 49 | % Outputs:
 50 | %       surfaceMeasurements = [timeStamp xPosition yPosition zPosition]
 51 | %       groundTruth = [timeStamp centerPosition' quaternions']
 52 | %           where centerPosition = [cX cY cZ] and quaternions = [q0 q1 q2 q3]
 53 | % Inputs:
 54 | %       stdMeas: standart deviation of a 3D point measurement on each axis
 55 | %       numInstants: number of instants included in the simulation
 56 | %       objType: enumerated variable, 1:Sphere, 2:Cube, 3:Ellipsoid,...
 57 | %       objParameters: depends on the object, i.e., for Cube it denotes edgeLength
 58 | %       experimentDuration: in seconds
 59 | %       T: time step between successive siulation instants (in seconds)
 60 | 
 61 | % Comment out one of the test scenarios below (A or B)
 62 | % A. Straight-line pattern:
 63 | % initialQuaternion = [0 ; 0; 0; 1];
 64 | % initialPosition = [0; 0; 0];
 65 | % linearVelocityMagnitude = 10; % in m/sec
 66 | % angularVelocityArray = zeros(numInstants, 3);
 67 | % angularVelocityPhi = 0;         % in rad/sec, around X axis of the local frame
 68 | % angularVelocityTheta = 0;   % in rad/sec, around Y axis of the local frame
 69 | % angularVelocityPsi = 0;    % in rad/sec, around Z axis of the local frame
 70 | % angularVelocityArray(:, 1) = angularVelocityPhi; 
 71 | % angularVelocityArray(:, 2) = angularVelocityTheta; 
 72 | % angularVelocityArray(31:230, 3) = angularVelocityPsi; 
 73 | 
 74 | % B. U-turn pattern:
 75 | initialQuaternion = [0; 0; 0; 1];
 76 | initialPosition = [0; 0; 0];
 77 | linearVelocityMagnitude = 0.5;          % in m/sec
 78 | angularVelocityArray = zeros(numInstants, 3);
 79 | angularVelocityPhi = pi/10;             % in rad/sec, around X axis of the local frame
 80 | angularVelocityTheta = pi/15;           % in rad/sec, around Y axis of the local frame
 81 | angularVelocityPsi = pi/20;             % in rad/sec, around Z axis of the local frame
 82 | angularVelocityArray(30:100, 1) = -angularVelocityPhi; 
 83 | angularVelocityArray(170:220, 1) = 0.7*angularVelocityPhi; 
 84 | angularVelocityArray(50:120, 2) = angularVelocityTheta; 
 85 | angularVelocityArray(190:250, 2) = -1*angularVelocityTheta; 
 86 | angularVelocityArray(80:150, 3) = angularVelocityPsi; 
 87 | angularVelocityArray(220:260, 3) = 0*angularVelocityPsi; 
 88 | 
 89 | %% Simulate the Motion
 90 | % Note that only kinematic aspects of motion is considered. No dynamic behaviour is introduced.
 91 | posArray = zeros(numInstants, 3); % : [xArray yArray zArray]
 92 | quatArray = zeros(numInstants, 4); % : [q0Array q1Array q2Array q3Array]
 93 | velArray = zeros(numInstants, 3); 
 94 | groundTruth = zeros(numInstants, 14); % : [timeStamp centerPosition' quaternions' angularRate' linearVel']
 95 | 
 96 | pos_k = initialPosition; % Initialize position
 97 | quat_k = normc(initialQuaternion); % Initialize quaternions
 98 | vel_k = linearVelocityMagnitude * [1; 0; 0];
 99 | angRate_k = angularVelocityArray(1,:)';
100 | 
101 | % Record the initial state
102 | posArray(1, :) = pos_k';
103 | velArray(1, :) = vel_k';
104 | quatArray(1, :) = quat_k';
105 | groundTruth(1, :) = [0 pos_k' quat_k' angRate_k' vel_k'];
106 | 
107 | for k = 1:(numInstants-1)
108 |     curTime = k*T;
109 |     
110 |     % Obtain transformation matrices
111 |     % First, find the linear velocity rotation matrix from local frame to the global
112 |     R_from_G_to_L = rotation_matrix_from_global_to_local(quat_k);
113 |     R_from_L_to_G = transpose(R_from_G_to_L);
114 |     % Then, find the matrix to transform local angular velocities into quaternion diffential
115 |     T_from_L_to_G = angular_velocity_transformation_matrix_from_local_to_global(quat_k);
116 |     
117 |     % Obtain the linear velocity vector expressed in the local frame
118 |     v_L_k = linearVelocityMagnitude * [1; 0; 0]; % It is assumed to be in X direction
119 |     
120 |     % Extract the current angular velocity vector
121 |     w_L_k = transpose(angularVelocityArray(k, :));
122 |     
123 |     % Convert the velocities into global frame
124 |     posDot_G_k = R_from_L_to_G * v_L_k;
125 |     %     quatDot_G_k = T_from_L_to_G * w_L_k;
126 |     quatDot_G_k = 1/2 * [quat_k(4)*w_L_k - cross(w_L_k, quat_k(1:3)); -w_L_k'*quat_k(1:3)];
127 |     
128 |     % Update the position and quaternions
129 |     pos_k = pos_k + T * posDot_G_k;
130 |     quat_k = quat_k + T * quatDot_G_k;
131 |     quat_k = normc(quat_k);
132 |     
133 |     % Record the variables
134 |     posArray(k+1, :) = pos_k';
135 |     velArray(k+1, :) = posDot_G_k';
136 |     quatArray(k+1, :) = quat_k';
137 |     groundTruth(k+1, :) = [curTime pos_k' quat_k' angularVelocityArray(k,:) posDot_G_k'];
138 | end
139 | 
140 | %% Produce surface measurements
141 | surfaceMeasurements = zeros(numInstants*numMeasPerInstant, 4); % Initialize the measurements
142 | for i = 1:numInstants
143 |     iTime = groundTruth(i, 1);
144 |     iPos = groundTruth(i, 2:4)';
145 |     iQuat = groundTruth(i, 5:8)';
146 |     
147 |     curMeasArray = produce_surface_measurements(iPos, iQuat, numMeasPerInstant, stdMeas, objType,...
148 |         objParameters);
149 |     
150 |     % Insert the measurements to the output matrix with relevant time stamp
151 |     surfaceMeasurements((i-1)*numMeasPerInstant + 1 : (i*numMeasPerInstant), :) = ...
152 |         [ones(numMeasPerInstant, 1)*iTime curMeasArray];
153 | end
154 | end
155 | 
156 | function [surfaceMeas] = produce_surface_measurements(pos, quat, numMeasurements, stdMeas, objType...
157 |     , objParameters)
158 | % PRODUCE_SURFACE_MEASUREMENTS generates 3D point measurements from the
159 | % surface of an object with specified geometry.
160 | switch objType
161 |     case 1 % Sphere
162 |         radius = objParameters;
163 |         
164 |         % Assume x,y and z are independent Gaussian random variables
165 |         sampleArray = randn(3, numMeasurements);
166 |         % Project each sample onto the sphere by normalizing the length
167 |         sampleArray = normc(sampleArray);
168 |         % Multiply the normalized sample to obtain desired radius
169 |         sampleArray = radius .* sampleArray;
170 |         
171 |         xTrue_L = sampleArray(1,:)';
172 |         yTrue_L = sampleArray(2,:)';
173 |         zTrue_L = sampleArray(3,:)';
174 |         
175 |     case 2 % Box
176 |         l = objParameters(1);
177 |         h = objParameters(2);
178 |         w = objParameters(3);
179 |         
180 |         % First, produce a random array for the selection of the faces
181 |         faceArray = randi([0 5], numMeasurements, 1);
182 |         
183 |         % Initialize the outputs
184 |         xTrue_L = zeros(numMeasurements, 1);
185 |         yTrue_L = zeros(numMeasurements, 1);
186 |         zTrue_L = zeros(numMeasurements, 1);
187 |         
188 |         % Select random points exactly on the specified box
189 |         pointer = 1;
190 |         for iFace = 0:5
191 |             num = sum(faceArray == iFace);
192 |             switch iFace
193 |                 case 0 % on X-Y plane, Z is positive
194 |                     xTrue_L(pointer:(pointer+num-1)) = -l/2 + l*rand(num, 1);
195 |                     yTrue_L(pointer:(pointer+num-1)) = -w/2 + w*rand(num, 1);
196 |                     zTrue_L(pointer:(pointer+num-1)) = h/2*ones(num,1);
197 |                 case 1 % on X-Y plane, Z is negative
198 |                     xTrue_L(pointer:(pointer+num-1)) = -l/2 + l*rand(num, 1);
199 |                     yTrue_L(pointer:(pointer+num-1)) = -w/2 + w*rand(num, 1);
200 |                     zTrue_L(pointer:(pointer+num-1)) = -h/2*ones(num,1);
201 |                 case 2 % on X-Z plane, Y is positive
202 |                     xTrue_L(pointer:(pointer+num-1)) = -l/2 + l*rand(num, 1);
203 |                     yTrue_L(pointer:(pointer+num-1)) = w/2*ones(num,1);
204 |                     zTrue_L(pointer:(pointer+num-1)) = -h/2 + h*rand(num, 1);
205 |                 case 3 % on X-Z plane, Y is negative
206 |                     xTrue_L(pointer:(pointer+num-1)) = -l/2 + l*rand(num, 1);
207 |                     yTrue_L(pointer:(pointer+num-1)) = -w/2*ones(num,1);
208 |                     zTrue_L(pointer:(pointer+num-1)) = -h/2 + h*rand(num, 1);
209 |                 case 4 % on Y-Z plane, X is positive
210 |                     xTrue_L(pointer:(pointer+num-1)) = l/2*ones(num,1);
211 |                     yTrue_L(pointer:(pointer+num-1)) = -w/2 + w*rand(num, 1);
212 |                     zTrue_L(pointer:(pointer+num-1)) = -h/2 + h*rand(num, 1);
213 |                 case 5 % on Y-Z plane, X is negative
214 |                     xTrue_L(pointer:(pointer+num-1)) = -l/2*ones(num,1);
215 |                     yTrue_L(pointer:(pointer+num-1)) = -w/2 + w*rand(num, 1);
216 |                     zTrue_L(pointer:(pointer+num-1)) = -h/2 + h*rand(num, 1);
217 |             end
218 |             pointer = pointer + num;
219 |         end   
220 | end
221 | 
222 | % Add Gaussian noise on selected points
223 | xNoisy_L = xTrue_L + stdMeas * randn(numMeasurements, 1);
224 | yNoisy_L = yTrue_L + stdMeas * randn(numMeasurements, 1);
225 | zNoisy_L = zTrue_L + stdMeas * randn(numMeasurements, 1);
226 | meas_L = [xNoisy_L'; yNoisy_L'; zNoisy_L'];
227 | 
228 | % Obtain the rotation matrix from local frame to the global
229 | R_from_G_to_L = rotation_matrix_from_global_to_local(quat);
230 | R_from_L_to_G = transpose(R_from_G_to_L);
231 | 
232 | % Rotate 3D points to the global frame
233 | meas_G = transpose(R_from_L_to_G * meas_L);
234 | 
235 | % Translate the measurements to the center of the object
236 | surfaceMeas = [meas_G(:,1)+pos(1) meas_G(:,2)+pos(2) meas_G(:,3)+pos(3)];
237 | end
238 | 
239 | function [ transformationMatrix ] = angular_velocity_transformation_matrix_from_local_to_global( quat )
240 | % The function computes the transformation matrix from Local (body) frame to the Global (inertial)
241 | % frame.
242 | % Note that this matrix is to multiply the angular velocity vector expressed in Local from from LEFT,
243 | % i.e., quatDot  =  transformationMatrix  *  wLocal
244 | 
245 | q0 = quat(1);
246 | q1 = quat(2);
247 | q2 = quat(3);
248 | q3 = quat(4);
249 | 
250 | % Rotation matrix from global to local R_from_G_to_L
251 | transformationMatrix = 1/2 * [-q1 -q2 -q3; q0 -q3 q2; q3 q0 -q1; -q2 q1 q0];
252 | end
253 | 


--------------------------------------------------------------------------------
/GPETTP/reconstruct_from_projections.m:
--------------------------------------------------------------------------------
 1 | function [recontructedObject3D] = reconstruct_from_projections(center3D, quat... 
 2 |     , extent1, extent2, extent3, basisAngleArray)
 3 | % This function reconstructs the 3D object from its three projected contours. 
 4 | % The 3D object is represented by 3D points. 
 5 | 
 6 | % Contour1 is on XY-plane, Contour2 is on XZ-plane, Contour3 is on YZ-plane of the local frame
 7 | P1 = [1 0 0; 0 1 0];    % Projects to Plane-1 (X-Y plane)
 8 | P2 = [1 0 0; 0 0 1];    % Projects to Plane-2 (X-Z plane)
 9 | P3 = [0 1 0; 0 0 1];    % Projects to Plane-3 (Y-Z plane)
10 | numBasisAngles = size(basisAngleArray, 1);
11 | margin = 0.5;           % Extention from the contour limits (in m)
12 | numLayers = 20;         % Number of test layers (the contours will be repeated for this many times)
13 | 
14 | % Determining Contour1 in Cartesian coordinates
15 | [x1, y1] = pol2cart(basisAngleArray, extent1);
16 | 
17 | % Determining Contour2 in Cartesian coordinates
18 | [x2, z2] = pol2cart(basisAngleArray, extent2);
19 | 
20 | % Determining Contour3 in Cartesian coordinates
21 | [y3, z3] = pol2cart(basisAngleArray, extent3);
22 | 
23 | % Construct the contours in Cartesian coordinates
24 | contour1 = [x1 y1; [x1(1) y1(1)]];  % Repeat the first vertex to close the loop
25 | contour2 = [x2 z2; [x2(1) z2(1)]];
26 | contour3 = [y3 z3; [y3(1) z3(1)]];
27 | 
28 | % Determine maximums and minimums from the 2D contours
29 | xmin = min([x1; x2]) - margin;
30 | xmax = max([x1; x2]) + margin;
31 | ymin = min([y1; y3]) - margin;
32 | ymax = max([y1; y3]) + margin;
33 | zmin = min([z2; z3]) - margin;
34 | zmax = max([z2; z3]) + margin;
35 | 
36 | % Build a linspaced array for each axis
37 | xArray = linspace(xmin, xmax, numLayers);
38 | xArray = repmat(xArray, numBasisAngles, 1);
39 | xArray = xArray(:);
40 | yArray = linspace(ymin, ymax, numLayers);
41 | yArray = repmat(yArray, numBasisAngles, 1);
42 | yArray = yArray(:);
43 | zArray = linspace(zmin, zmax, numLayers);
44 | zArray = repmat(zArray, numBasisAngles, 1);
45 | zArray = zArray(:);
46 | 
47 | % Build a rough shape by repeated contours
48 | contour1Repeated = [repmat(x1, numLayers, 1) repmat(y1, numLayers, 1) zArray];
49 | contour2Repeated = [repmat(x2, numLayers, 1) yArray repmat(z2, numLayers, 1)];
50 | contour3Repeated = [xArray repmat(y3, numLayers, 1) repmat(z3, numLayers, 1)];
51 | 
52 | testPoints = [contour1Repeated; contour2Repeated; contour3Repeated];
53 | 
54 | %% Test points are to be eleminated by the following carving process
55 | % Plane 1
56 | testPointsProjected = P1 * testPoints';   % Project all points on Plane1
57 | testPointsProjected = testPointsProjected';
58 | % Delete the points that are outside Contour1
59 | in = inpolygon(testPointsProjected(:, 1), testPointsProjected(:, 2), contour1(:,1), contour1(:,2)); 
60 | testPoints(~in, :) = []; 
61 | 
62 | % Plane 2
63 | testPointsProjected = P2 * testPoints';   % Project all points on Plane2
64 | testPointsProjected = testPointsProjected';
65 | % Delete the points that are outside Contour2
66 | in = inpolygon(testPointsProjected(:, 1), testPointsProjected(:, 2), contour2(:,1), contour2(:,2)); 
67 | testPoints(~in, :) = []; 
68 | 
69 | % Plane3
70 | testPointsProjected = P3 * testPoints';
71 | testPointsProjected = testPointsProjected';
72 | % Delete the points that are outside Contour3
73 | in = inpolygon(testPointsProjected(:, 1), testPointsProjected(:, 2), contour3(:,1), contour3(:,2)); 
74 | testPoints(~in, :) = []; 
75 | 
76 | % Rotate the points regarding the orientation of the 3D object
77 | R_from_G_to_L = rotation_matrix_from_global_to_local(quat);
78 | R_from_L_to_G = transpose(R_from_G_to_L);
79 | testPoints_G = transpose(R_from_L_to_G * testPoints');
80 | 
81 | % Shift the points to the object center
82 | recontructedObject3D = testPoints_G + center3D'; 
83 | 
84 | end


--------------------------------------------------------------------------------
/GPETTP/rotation_matrix_from_global_to_local.m:
--------------------------------------------------------------------------------
 1 | function [ rotMatrix ] = rotation_matrix_from_global_to_local( quat )
 2 | % The function computes the rotation matrix from the global frame to the Local (body)
 3 | % frame.
 4 | % Note that the rotation matrix is to multiply the vector from LEFT,
 5 | % i.e., xL  =  rotMatrix  *  xG.
 6 | 
 7 | quat = quat(:);     % Makes sure the input is a column matrix
 8 | 
 9 | q = quat(1:3);
10 | q4 = quat(4);
11 | 
12 | % Rotation matrix from global to local R_from_G_to_L
13 | rotMatrix = transpose((q4^2-q'*q)*eye(3,3) + 2*(q*q') - 2*q4*skew_symmetric_matrix(q));
14 | end
15 | 
16 | function [out] = skew_symmetric_matrix(x)
17 | out = [0  -x(3)  x(2);...
18 |     x(3)  0  -x(1);...
19 |     -x(2)  x(1)  0];
20 | end
21 | 


--------------------------------------------------------------------------------
/GPETTP/visualize_tracking_outputs.m:
--------------------------------------------------------------------------------
  1 | function [  ] = visualize_tracking_outputs( estState, estQuat, estStateVar, measArray...
  2 |     , groundTruth, time, basisAngleArray )
  3 | % VISUALIZE_TRACKING_OUTPUTS visualizes the 3D extent estimate with the
  4 | % associated uncertainty information. Besides, if the ground truth is
  5 | % available, it is plotted as well.
  6 | 
  7 | % Determine whether the ground truth information is provided
  8 | if isempty(groundTruth)
  9 |     isGTAvailable = 0;
 10 | else
 11 |     isGTAvailable = 1;
 12 | end
 13 | 
 14 | % Extract relevant information from the updated state
 15 | estCenter = estState(1:3);
 16 | % estVelocity = estState(4:6);
 17 | numBasisAngles = size(basisAngleArray,1);
 18 | estExtent_P1 = estState(13:12+numBasisAngles);
 19 | estExtent_P2 = estState(13+numBasisAngles:12+2*numBasisAngles);
 20 | estExtent_P3 = estState(13+2*numBasisAngles:12+3*numBasisAngles);
 21 | 
 22 | % Extract extent variances
 23 | stdState = sqrt(estStateVar);
 24 | stdExtent_P1 = stdState(13:12+numBasisAngles);
 25 | stdExtent_P2 = stdState(13+numBasisAngles:12+2*numBasisAngles);
 26 | stdExtent_P3 = stdState(13+2*numBasisAngles:12+3*numBasisAngles);
 27 | 
 28 | % Extract ground truth values of the position and quaternions
 29 | if isGTAvailable
 30 |     objType = groundTruth.objectDescription(1);
 31 |     objParam = groundTruth.objectDescription(2:end);
 32 |     gtKinematics = groundTruth.dataLog(abs(groundTruth.dataLog(:,1)-time)<1e-10, 2:end);
 33 |     gtCenter =gtKinematics(1:3)';
 34 |     gtQuat = gtKinematics(4:7)';
 35 | end
 36 | 
 37 | % Contruct the 3D object from the projection contours
 38 | reconstructedObject3D = reconstruct_from_projections(estCenter, estQuat...
 39 |     , estExtent_P1, estExtent_P2, estExtent_P3, basisAngleArray);
 40 | 
 41 | cla(); % clear the current axis
 42 | 
 43 | % Plot the point measurements
 44 | plot3(measArray(:,1), measArray(:,2), measArray(:,3), 'r+', 'LineWidth', 2, 'MarkerSize', 5);
 45 | 
 46 | % Plot reconstructed object
 47 | plot_reconstructed_3D_object(reconstructedObject3D);
 48 | 
 49 | % Plot the ground truth
 50 | if isGTAvailable
 51 |     switch objType
 52 |         case 1 % Sphere
 53 |             gTPlotHandle = plot_sphere(gtCenter, objParam, [1 0 1]);
 54 |         case 2 % Box
 55 |             gTPlotHandle = plot_box(gtCenter, gtQuat, objParam, [1 0 1]);
 56 |     end
 57 | end
 58 | 
 59 | % Attach a legend
 60 | legend('Measurements', 'Estimated surface', 'Ground truth');
 61 | 
 62 | % Arrange lighting in the environment
 63 | delete(findall(gcf,'Type','light'))
 64 | lightangle(-60,60)
 65 | lighting phong;
 66 | material shiny;
 67 | drawnow();
 68 | end
 69 | 
 70 | function plotHandles = plot_box(origin, quaternions, objectParameters, color)
 71 | 
 72 | % Extract object parameters
 73 | length = objectParameters(1);
 74 | height = objectParameters(2);
 75 | width = objectParameters(3);
 76 | 
 77 | % Define the vertices of the box
 78 | vertices_L = [length width -height;
 79 |     -length width -height;
 80 |     -length width height;
 81 |     length width height;
 82 |     -length -width height;
 83 |     length -width height;
 84 |     length -width -height;
 85 |     -length -width -height]* 0.5;
 86 | 
 87 | R_from_G_to_L = rotation_matrix_from_global_to_local(quaternions);
 88 | R_from_L_to_G = transpose(R_from_G_to_L);
 89 | vertices_G = transpose(R_from_L_to_G * vertices_L');
 90 | 
 91 | %  Define the faces of the box
 92 | fac = [1 2 3 4;
 93 |     4 3 5 6;
 94 |     6 7 8 5;
 95 |     1 2 8 7;
 96 |     6 7 1 4;
 97 |     2 3 5 8];
 98 | box = [vertices_G(:,1)+origin(1),vertices_G(:,2)+origin(2),vertices_G(:,3)+origin(3)];
 99 | 
100 | plotHandles = patch('Faces',fac,'Vertices',box,'FaceColor',color, 'EdgeColor'...
101 |     , [0.3 0.3 0.3], 'LineWidth', 1,'FaceAlpha', 0);
102 | end
103 | 
104 | function plotHandles = plot_sphere(center, objectParameters, color)
105 | % Plots a sphere
106 | 
107 | % Extract object parameters
108 | radius = objectParameters;
109 | 
110 | % Produce cartesian points on the unit sphere
111 | [xSphere, ySphere, zSphere] = sphere(128);
112 | 
113 | % Scale and translate these points
114 | xSphere = xSphere*radius + center(1);
115 | ySphere = ySphere*radius + center(2);
116 | zSphere = zSphere*radius + center(3);
117 | 
118 | % Plot the surface
119 | h = surfl(xSphere, ySphere, zSphere);
120 | h.FaceColor = color;
121 | h.FaceAlpha = 0.3;
122 | h.EdgeColor = 'none';
123 | 
124 | % Return the surface handle
125 | plotHandles = h;
126 | end
127 | 
128 | function [ plotHandles ] = plot_reconstructed_3D_object( object3DPoints )
129 | 
130 | triangles = boundary(object3DPoints, 0.5);                                      % perform triangulation
131 | s = trisurf(triangles, object3DPoints(:,1), object3DPoints(:,2)...
132 |     , object3DPoints(:,3), 'FaceAlpha', 0.6, 'FaceColor', [0.3 0.75 0.93]);     % plot the triangulated surface
133 | s.EdgeColor = 'none';
134 | 
135 | plotHandles = s;
136 | 
137 | end
138 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | This repository presents an example implementation of the algorithms proposed in the following paper.
 2 | 
 3 | "Three-Dimensional Extended Object Tracking and Shape Learning Using Gaussian Processes" (IEEE Trans. Aerosp.  Electron. Syst. , vol. 57, pp. 2795–2814, Mar. 2021) by M. Kumru and E. Özkan
 4 | DOI: 10.1109/TAES.2021.3067668
 5 | URL: https://ieeexplore.ieee.org/abstract/document/9382878
 6 | 
 7 | This repository consists of two separate folders: 
 8 | - GPETT:  GP-based extended target tracker (proposed in Section V of the mentioned paper)
 9 | - GPETTP: GP-based extended target tracker using projections (proposed in Section VII of the mentioned paper)
10 | 
11 | 


--------------------------------------------------------------------------------