├── LICENSE ├── README.md ├── addIfNotOnPath.m ├── model └── variationalRadarModel.mat ├── plotVariationalRadarModel.m └── src ├── getConditionalTMixture.m ├── getMarginalTMixture.m ├── plot2DStudentTMixture.m └── studentTMixturePDF.m /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Alexander Scheel 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Variational Radar Model 2 | 3 | This repository provides MATLAB code that accompanies the paper "Tracking Multiple Vehicles Using a Variational Radar Model" by Alexander Scheel and Klaus Dietmayer. See below for the full citation. The variational radar model provides a probabilistic model of radar measurements (i.e. radar detections) from vehicles. It can be used for tracking other vehicles in autonomous driving and driver assistance system applications. The repository contains the model itself as well as scripts for ploting different densities that can be deduced. Please refer to the paper for a detailed description of the model. 4 | 5 | Paper reference: 6 | Alexander Scheel and Klaus Dietmayer, "Tracking Multiple Vehicles Using a Variational Radar Model", in IEEE Transactions on Intelligent Transportation Systems, vol. 20, no. 10, pp. 3721-3736, Oct. 2019. 7 | 8 | ## Prerequisites 9 | 10 | As the code is implemented in MATLAB, an installed MATLAB version is necessary. The code has been tested for the MATLAB versions R2016a and R2017a. 11 | 12 | ## Structure and Usage 13 | 14 | The variational radar model itself is provided as .mat-File in the folder model/. It is given as four-dimensional Student's t mixture where the dimensions are 15 | 1: x' - aspect angle under which the sensor sees the vehicle, 16 | 2: z'_x - x-coordinate of the measurement in object coordinates, 17 | 3: z'_y - y-coordinate of the measurement in object coordinates, 18 | 4: z'_d - Doppler error. 19 | 20 | The mixture density is a struct with fields 21 | roh - vector of mixing coefficients, 22 | gamma - 4x50 matrix where the i-th column contains the mean vector of the i-th Student's t density, 23 | nu - vector of degrees of freedom, the i-th entry corresponds to the i-th component, 24 | Htilde - 4x4x50 matrix where the i-th slice contains the precision matrix of the i-th component. 25 | 26 | The script plotVariationalRadarModel.m creates exemplary views of the model by computing different marginal and conditional densities. 27 | 28 | Functions for computing marginal and conditional densities as well as for evaluating the Student's t mixtures are stored in the folder src/. 29 | 30 | ## License and Citations 31 | 32 | This project is licensed under the MIT License - see the [LICENSE.md](LICENSE.md) file for details. 33 | 34 | Please cite our original paper if you use our code for your research. 35 | 36 | @misc{Scheel.2019, 37 | author = {Scheel, Alexander and Dietmayer, Klaus}, 38 | journal={IEEE Transactions on Intelligent Transportation Systems}, 39 | title={Tracking Multiple Vehicles Using a Variational Radar Model}, 40 | year={2019}, 41 | month={Oct}, 42 | volume={20}, 43 | number={10}, 44 | pages={3721-3736} 45 | } 46 | -------------------------------------------------------------------------------- /addIfNotOnPath.m: -------------------------------------------------------------------------------- 1 | %ADDIFNOTONPATH Adds folder to path if it is not already on it 2 | % 3 | % ADDIFNOTONPATH(folder) adds folder to the MATLAB path if it is not 4 | % already on the path 5 | % 6 | % Author: Alexander Scheel (copied from thread on mathworks.com) 7 | 8 | function addIfNotOnPath(folder) 9 | 10 | % check if the folder is already on the path 11 | pathCell = regexp(path, pathsep, 'split'); 12 | if ispc 13 | onPath = any(strcmpi(folder, pathCell)); 14 | else 15 | onPath = any(strcmp(folder, pathCell)); 16 | end 17 | 18 | % add it if it is not on the path 19 | if ~onPath 20 | addpath(folder); 21 | end 22 | 23 | end -------------------------------------------------------------------------------- /model/variationalRadarModel.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/A-Scheel/Variational-Radar-Model/491cab8d653f1343ea9880caa762358564654d63/model/variationalRadarModel.mat -------------------------------------------------------------------------------- /plotVariationalRadarModel.m: -------------------------------------------------------------------------------- 1 | %PLOTVARIATIONALRADARMODEL Script for plotting the variational radar model 2 | % 3 | % PLOTVARIATIONALRADARMODEL plots the variational model in six different 4 | % perspectives. It generates the marginal density over the location of 5 | % measurements, four densities that are conditioned on different aspect 6 | % angles, and a conditional density that shows the Doppler errors. 7 | % 8 | % Author: Alexander Scheel 9 | 10 | clear all; 11 | close all; 12 | 13 | % add the source folder to the MATLAB path 14 | [currPath, ~, ~] = fileparts(mfilename('fullpath')); 15 | addIfNotOnPath([currPath, filesep, 'src']); 16 | 17 | %% load the variational radar model 18 | % Loading the variational radar model creates the variable 19 | % jointPredicitveDensity which contains the predictive Student's t mixture 20 | % for the joint density of transformed measurements and aspect angle. 21 | % 22 | % The mixture is stored as a struct with the following fields 23 | % jointPredictiveDensity.roh - mixing coefficients 24 | % jointPredictiveDensity.gamma - 4x50 matrix where the i-th column contains 25 | % the mean vector of the i-th Student's t 26 | % density 27 | % jointPredictiveDensity.nu - vector of degrees of freedom for each 28 | % component 29 | % jointPredictiveDensity.Htilde - 4x4x50 matrix where the i-th slice 30 | % contains the precision matrix of the i-th 31 | % component 32 | % 33 | % The density is four-dimensional with the order 34 | % 1: x' - x-coordinate of the measurement in normalized object coordinates 35 | % 2: z'_x - x-coordinate of the measurement in normalized object coordinates 36 | % 3: z'_y - y-coordinate of the measurement in normalized object coordinates 37 | % 4: z'_d - Doppler error 38 | disp('Loading the model ...') 39 | load(['model', filesep, 'variationalRadarModel.mat']) 40 | 41 | %% plot of the marginal density p(z'_x, z'_y) 42 | disp('Plotting marginal density p(z''_x, z''_y) ...') 43 | 44 | % get the marginal density for indices 2 and 3 (z'_x and z'_y) 45 | margDensity = getMarginalTMixture(jointPredictiveDensity, [2, 3]); 46 | 47 | % figure settings 48 | figSettings = []; 49 | figSettings.title = 'Marginal Density p(z''_x, z''_y)'; 50 | figSettings.axis = [-0.5, 1 -1, 1]; 51 | figSettings.daspect = [1/5, 1/2, 1]; 52 | % create a contour plot 53 | plot2DStudentTMixture(margDensity, figSettings); 54 | 55 | %% plot conditioanl densities p(z'_x, z'_y | x') 56 | disp('Plotting conditional densities p(z''_x, z''_y | x'') ...') 57 | 58 | % get the marginal density p(z'_x, z'_y, x') first (i.e. marginalize over 59 | % Doppler) 60 | margWoDoppler = getMarginalTMixture(jointPredictiveDensity, [1, 2, 3]); 61 | 62 | % get and plot the conditional density for x' = -3; 63 | condDensity = getConditionalTMixture(margWoDoppler, 1, -3); 64 | figSettings = []; 65 | figSettings.title = 'Conditional Density p(z''_x, z''_y | x'' = -3)'; 66 | figSettings.axis = [-0.5, 1 -1, 1]; 67 | figSettings.daspect = [1/5, 1/2, 1]; 68 | figSettings.minContourValue = 0.4; 69 | plot2DStudentTMixture(condDensity, figSettings); 70 | 71 | % get and plot the conditional density for x' = -pi/2; 72 | condDensity = getConditionalTMixture(margWoDoppler, 1, -pi/2); 73 | figSettings = []; 74 | figSettings.title = 'Conditional Density p(z''_x, z''_y | x'' = -\pi/2)'; 75 | figSettings.axis = [-0.5, 1 -1, 1]; 76 | figSettings.daspect = [1/5, 1/2, 1]; 77 | figSettings.minContourValue = 0.4; 78 | plot2DStudentTMixture(condDensity, figSettings); 79 | 80 | % get and plot the conditional density for x' = -pi/2; 81 | condDensity = getConditionalTMixture(margWoDoppler, 1, -pi/4); 82 | figSettings = []; 83 | figSettings.title = 'Conditional Density p(z''_x, z''_y | x'' = -\pi/4)'; 84 | figSettings.axis = [-0.5, 1 -1, 1]; 85 | figSettings.daspect = [1/5, 1/2, 1]; 86 | figSettings.minContourValue = 0.4; 87 | plot2DStudentTMixture(condDensity, figSettings); 88 | 89 | % get and plot the conditional density for x' = -pi/2; 90 | condDensity = getConditionalTMixture(margWoDoppler, 1, 0); 91 | figSettings = []; 92 | figSettings.title = 'Conditional Density p(z''_x, z''_y | x'' = 0)'; 93 | figSettings.axis = [-0.5, 1 -1, 1]; 94 | figSettings.daspect = [1/5, 1/2, 1]; 95 | figSettings.minContourValue = 0.4; 96 | plot2DStudentTMixture(condDensity, figSettings); 97 | 98 | %% plot conditioanl densities p(z'_x, z'_d | z'_y = -0.5, x' = -\pi/2) 99 | disp('Plotting conditional density p(z''_x, z''_d | z''_y = -0.5, x'' = -\pi/2) ...') 100 | 101 | condDensity = getConditionalTMixture(jointPredictiveDensity, [1, 3], [-pi/2, -0.5]); 102 | figSettings = []; 103 | figSettings.title = 'Conditional Density p(z''_x, z''_d | z''_y = -0.5, x'' = \pi/2)'; 104 | figSettings.axis = [-0.5, 1 -1.5, 1.5]; 105 | figSettings.daspect = [1/5, 1/2, 1]; 106 | plot2DStudentTMixture(condDensity, figSettings); -------------------------------------------------------------------------------- /src/getConditionalTMixture.m: -------------------------------------------------------------------------------- 1 | %GETCONDITIONALTMIXTURE Get conditional Student's t mixture 2 | % 3 | % cond = GETCONDITIONALTMIXTURE(model, condIdcs, condVals) computes the 4 | % Student's t mixture density that results from conditioning the 5 | % Student's t mixture density model on part of the random variable. The 6 | % indices of the vector components that we condition on are specified in 7 | % condIdcs and their values in condVals. 8 | % 9 | % The procedure is derived from the equations for a conditional Student's 10 | % t distribution in [1] and extending them to a mixture density through 11 | % p(x, z) \sum_i( w_i * p_i(x,z) ) 12 | % p(x|z) = ------- = ------------------------ = 13 | % p(z) \sum_j( w_j * p_j(z) ) 14 | % 15 | % w_i * p_i(z) 16 | % = \sum_i( ----------------------- * p_i(x|z) ) 17 | % \sum_j( w_j * p_j(z) ) 18 | % 19 | % [1] Roth, M.: "On the Multivariate t Distribution". Linköpinbg, Sweden, 20 | % 2013 21 | % 22 | % Example 23 | % load(['model', filesep, 'variationalRadarModel.mat']); 24 | % cond = getConditionalTMixture(jointPredictiveDensity, ... 25 | % [1, 3], [-pi/2, -0.5]) 26 | % 27 | % The example computes the conditional density, when conditioning on an 28 | % aspect angle of -pi/2 and a y-position of -0.5. 29 | % 30 | % Author: Alexander Scheel 31 | 32 | function cond = getConditionalTMixture(model, condIdcs, condVals) 33 | 34 | %% preliminary checks and conversions 35 | % get the number of components 36 | nComponents = numel(model.rho); 37 | 38 | % bring condIdcs in the correct format 39 | if ismatrix(condIdcs) || ~any(size(condIdcs) == 1) 40 | if size(condIdcs,1) < size(condIdcs,2) 41 | condIdcs = condIdcs'; 42 | end 43 | else 44 | error('The condIdcs vector needs to be a column or row vector!') 45 | end 46 | 47 | % bring condIdcs in the correct format 48 | if ismatrix(condVals) || ~any(size(condVals) == 1) 49 | if size(condVals,1) < size(condVals,2) 50 | condVals = condVals'; 51 | end 52 | else 53 | error('The condIdcs vector needs to be a column or row vector!') 54 | end 55 | 56 | % get the dimension of the joint density 57 | dimJoint = size(model.gamma,1); 58 | 59 | %% rearrange the density 60 | % The density is rearranged such that the values that the mixture is 61 | % conditioned on are at the end. Thus, the euqations of [1] are applicable. 62 | 63 | % find the remaining indices 64 | remainingIdcs = find(~any(bsxfun(@eq, 1:dimJoint, condIdcs),1)); 65 | 66 | % get the number of remaining indices (there must be at least one remaining 67 | % index) 68 | nRemaining = numel(remainingIdcs); 69 | if isempty(remainingIdcs) 70 | error('There must be at least one remaining variable for computing the conditional density!'); 71 | end 72 | 73 | % construct a sort vector that puts the remaining indices first 74 | sortVec = [remainingIdcs'; condIdcs]; 75 | 76 | % rearrange dimensions 77 | newGamma = model.gamma(sortVec,:); 78 | newHTilde = model.Htilde(sortVec,sortVec,:); 79 | 80 | %% compute the parameters of the conditional density 81 | % compute the difference between the conditioned value and the mean 82 | delta = bsxfun(@minus, condVals, newGamma(nRemaining+1:end,:)); 83 | 84 | % compute the mean vectors and precision matrices of the conditional 85 | % density following [1] 86 | condGamma = zeros(nRemaining,nComponents); 87 | condHTilde = zeros(nRemaining,nRemaining,nComponents); 88 | for i = 1:nComponents 89 | % convert the precision matrices to covariance matrices 90 | newSigma = inv(newHTilde(:,:,i)); 91 | 92 | % split the covariance matrix 93 | sigma11 = newSigma(1:nRemaining,1:nRemaining); 94 | sigma12 = newSigma(1:nRemaining,nRemaining+1:end); 95 | sigma22 = newSigma(nRemaining+1:end,nRemaining+1:end); 96 | 97 | % compute the new mean vector 98 | condGamma(:,i) = newGamma(1:nRemaining,i) + sigma12 * (sigma22 \ delta(:,i)); 99 | 100 | % compute the new covariance matrix 101 | condSigma = (model.nu(i) + delta(:,i)' * (sigma22 \ delta(:,i))) / ... 102 | (model.nu(i) + dimJoint - nRemaining) * ... 103 | (sigma11 - sigma12 * (sigma22 \ sigma12')); 104 | condHTilde(:,:,i) = inv(condSigma); 105 | end 106 | 107 | % get the marginal density of the value that is conditioned on and evaluate 108 | % it 109 | margDensity = getMarginalTMixture(model, condIdcs); 110 | [p, pComp] = studentTMixturePDF(margDensity, condVals); 111 | pComp = permute(pComp, [1, 3, 2]); 112 | 113 | % assemble the conditional density and compute the new weights 114 | cond.rho = model.rho .* pComp / p; 115 | cond.gamma = condGamma; 116 | cond.Htilde = condHTilde; 117 | cond.nu = model.nu + dimJoint - nRemaining; 118 | 119 | end 120 | 121 | -------------------------------------------------------------------------------- /src/getMarginalTMixture.m: -------------------------------------------------------------------------------- 1 | %GETMARGINALTMIXTURE Get marginal Student's t mixture 2 | % 3 | % marg = GETMARGINALTMIXTURE(model, margIdcs) outputs the marginal 4 | % density marg from the joint density model. The vector margIdcs 5 | % specifies which entries are part of the marginal density. 6 | % 7 | % Example 8 | % load(['model', filesep, 'variationalRadarModel.mat']); 9 | % marg = getMarginalTMixture(jointPredictiveDensity, [2 3]) 10 | % 11 | % This example loads the radar model and computes the marginal 12 | % distribution over the dimensions 2 and 3, i.e. the x- and y-coordinates 13 | % of the measurements. 14 | % 15 | % Author: Alexander Scheel 16 | 17 | function marg = getMarginalTMixture(model, margIdcs) 18 | 19 | % get the number of components in the mixture 20 | nComponents = numel(model.rho); 21 | 22 | % get the dimension of the marginal density 23 | dimMarg = numel(margIdcs); 24 | 25 | % the mixture coefficients and degrees of freedom stay the same 26 | marg.rho = model.rho; 27 | marg.nu = model.nu; 28 | 29 | % reduce the mean vectors 30 | marg.gamma = model.gamma(margIdcs,:); 31 | 32 | % initialize precision matrices 33 | marg.Htilde = zeros(dimMarg, dimMarg, nComponents); 34 | 35 | % iterate over precision matrices and compute marginal preceision matrices 36 | for i = 1:nComponents 37 | % make the precision matrices a covariance matrix 38 | sigma = inv(model.Htilde(:,:,i)); 39 | 40 | % select the necessary components 41 | sigmaPart = sigma(margIdcs,margIdcs); 42 | 43 | % make it a precision matrix again 44 | marg.Htilde(:,:,i) = inv(sigmaPart); 45 | end 46 | 47 | end -------------------------------------------------------------------------------- /src/plot2DStudentTMixture.m: -------------------------------------------------------------------------------- 1 | %PLOT2DSTUDENTTMIXTURE Contour plot of a two-dimensional Student's t 2 | % mixture 3 | % 4 | % fig = PLOT2DSTUDENTTMIXTURE(model) creates a contour plot of the 5 | % two-dimensional Student'S t mixture model. It outputs the figure handle 6 | % fig. 7 | % 8 | % fig = PLOT2DSTUDENTTMIXTURE(model, settings) allows to additional 9 | % specify some plot parameters through the argument settings. It is a 10 | % struct that may contain the following fields (default values and the 11 | % format are given in brackets): 12 | % 13 | % settings.title (default value: '') - Title of the figure 14 | % settings.axis (default value: [-1 1 -1 1]) - Axis limits as [xmin, 15 | % xmax, ymin, ymax] 16 | % settings.daspect (default value: [1 1 1]) - Data aspect ratio of the 17 | % axis 18 | % settings.minContourValue (default value: 0.1) - Value of the smallest 19 | % contour in the plot 20 | % 21 | % Author: Alexander Scheel 22 | 23 | function fig = plot2DStudentTMixture(model, settings) 24 | 25 | %% settings handling 26 | % create the settings variable if it has not been passed 27 | if nargin <= 1 28 | settings = []; 29 | end 30 | 31 | % default settings 32 | defaultSettings.title = ''; 33 | defaultSettings.axis = [-1 1 -1 1]; 34 | defaultSettings.daspect = [1 1 1]; 35 | defaultSettings.minContourValue = 0.1; 36 | 37 | % replace missing settings by the default values 38 | fields = fieldnames(defaultSettings); 39 | for i = 1:numel(fields) 40 | if ~isfield(settings, fields{i}) 41 | settings.(fields{i}) = defaultSettings.(fields{i}); 42 | end 43 | end 44 | 45 | %% create the figure 46 | % create figure 47 | fig = figure('name', settings.title); 48 | grid on; 49 | box on; 50 | hold on; 51 | ax = gca; 52 | axis(settings.axis); 53 | title(settings.title); 54 | daspect(settings.daspect); 55 | 56 | % create meshgrid for contour plot 57 | gridSpacingA = (settings.axis(2) - settings.axis(1))/500; 58 | gridSpacingB = (settings.axis(4) - settings.axis(3))/500; 59 | [a, b] = meshgrid(settings.axis(1):gridSpacingA:settings.axis(2), ... 60 | settings.axis(3):gridSpacingB:settings.axis(4)); 61 | meshPoints = [a(:), b(:)]'; 62 | 63 | % compute the density values for the mesh points 64 | p = studentTMixturePDF(model, meshPoints); 65 | 66 | % define the levels of the contour plot in dependence on the maximum 67 | % density value 68 | pMax = max(p); 69 | if settings.minContourValue > pMax 70 | warning(['The minimum contour value is above the maximum density value.', ... 71 | ' Choosing backup settings instead.']) 72 | levels = linspace(pMax*0.1, pMax, 8); 73 | else 74 | levels = linspace(settings.minContourValue, pMax, 8); 75 | end 76 | 77 | % create a contour plot 78 | contour(ax, a, b, reshape(p, size(a)), levels); 79 | 80 | % draw color bar 81 | cb = colorbar; 82 | set(get(cb, 'label'), 'string', 'Density Value') 83 | end -------------------------------------------------------------------------------- /src/studentTMixturePDF.m: -------------------------------------------------------------------------------- 1 | %STUDENTTMIXTUREPDF Computes the pdf values for given samples and a 2 | % student's t mixture model 3 | % 4 | % p = STUDENTTMIXTUREPDF(model, data) outputs pdf values for the 5 | % Student's t mixture that is defined in model. The points for which the 6 | % density values are computed are given in data as a matrix. The matrix 7 | % dimensions are 'Dimension of the model' x 'Number of data points'. 8 | % 9 | % [p, pComp] = STUDENTTMIXTUREPDF(model, data) additionally outputs the 10 | % density value of each component individually in pComp. 11 | % 12 | % Author: Alexander Scheel 13 | 14 | function [p, pComp] = studentTMixturePDF(model, data) 15 | 16 | % get dimension of data and get the number of data points 17 | [D, nData] = size(data); 18 | 19 | % get number of components 20 | nComp = numel(model.rho); 21 | 22 | % pre-compute the determinants 23 | if D > 1 24 | d = zeros(1,1,nComp); 25 | for i = 1:nComp 26 | d(i) = det(model.Htilde(:,:,i)); 27 | end 28 | else 29 | d = model.Htilde; 30 | end 31 | 32 | % get the nus 33 | nu = permute(model.nu, [1,3,2]); 34 | 35 | % compute the gamma factor 36 | gammaFactor = exp(gammaln(nu/2 + D/2) - gammaln(nu/2)); 37 | 38 | % compute the determinant factor 39 | detFactor = sqrt(d) ./ (nu * pi).^(D/2); 40 | 41 | % compute the delta 42 | mu = permute(model.gamma, [1,3,2]); 43 | delta = bsxfun(@minus, data, mu); 44 | HtildeTimesDelta = zeros(D,nData,nComp); 45 | for i = 1:D 46 | HtildeTimesDelta(i,:,:) = sum(bsxfun(@times, permute(model.Htilde(i,:,:), [2, 1, 3]), delta),1); 47 | end 48 | delta2 = sum(delta .* HtildeTimesDelta,1); 49 | 50 | % compute the delta factor 51 | deltaFactor = bsxfun(@power, (1 + bsxfun(@rdivide, delta2, nu)), (-nu/2-D/2)); 52 | 53 | % get the mixing coefficients 54 | mix = permute(model.rho, [1,3,2]); 55 | 56 | % get the overall pdf value 57 | pComp = bsxfun(@times, gammaFactor .* detFactor, deltaFactor); 58 | p = sum(bsxfun(@times, pComp, mix),3); 59 | 60 | end --------------------------------------------------------------------------------