Particle filtering based 3D multi-target tracker (SAF_TRACKER_MODULE) More...

#include "saf_tracker.h"
#include "saf_tracker_internal.h"

Data Structures
struct	kf_update6_data
	Data structure for kf_update6() More...

Macros
#define	SAF_LOG_2PI ( logf(2.0f*SAF_PI) )
	log(2pi)

Functions
static double	lngamma (double x, double *sgngam)
	Natural logarithm of gamma function.

static double	incompletegamma (double a, double x)
	Incomplete gamma integral.

static double	incompletegammac (double a, double x)
	Complemented incomplete gamma integral.

void	tracker3d_particleCreate (void **phPart, float W0, float dt)
	Creates an instance of a particle / Monte-Carlo Sample.

void	tracker3d_particleReset (void *hPart)
	Resets a particle structure to defaults.

void	tracker3d_particleCopy (void hPart1, void hPart2)
	Copies particle structure "hPart1" into structure "hPart2".

void	tracker3d_particleDestroy (void **phPart)
	Destroys an instance of a particle / Monte-Carlo Sample.

void	tracker3d_predict (void *const hT3d, int Tinc)
	Prediction step.

void	tracker3d_update (void const hT3d, float Y, int Tinc)
	Prediction update.

int	tracker3d_getMaxParticleIdx (void *const hT3d)
	Returns the index of the most important particle.

void	resampstr (voidPtr SS, int NP, int s)
	Stratified resampling - returns a new set of indices according to the probabilities P.

float	eff_particles (voidPtr *SS, int NP)
	Estimate the number of effective particles.

void	normalise_weights (voidPtr *SS, int NP)
	Normalises the weights of the given particles.

void	kf_predict6 (float M[6], float P[6][6], float A[6][6], float Q[6][6])
	Perform Kalman Filter prediction step.

void	kf_update6_create (void **const phUp6)
	Creates helper structure for kf_update6()

void	kf_update6_destroy (void **const phUp6)
	Destroys helper structure for kf_update6()

void	kf_update6 (void const hUp6, float X[6], float P[6][6], float y[3], float H[3][6], float R[3][3], float X_out[6], float P_out[6][6], float LH)
	Kalman Filter update step.

float	gamma_cdf (float x, float gam, float beta, float mu)
	Cumulative density function of a Gamma distribution.

void	lti_disc (float F, int len_N, int len_Q, float opt_L, float opt_Qc, float dt, float A, float *Q)
	LTI_DISC Discretize LTI ODE with Gaussian Noise.

float	gauss_pdf3 (void *const hUp6, float X[3], float M[3], float S[3][3])
	Multivariate Gaussian PDF.

int	categ_rnd (float *P, int len_P)
	Draws samples from a given one dimensional discrete distribution.

Detailed Description

Particle filtering based 3D multi-target tracker (SAF_TRACKER_MODULE)

More information regarding this specific implementation can be found in [2]

See also: [1] Sa"rkka", S., Vehtari, A. and Lampinen, J., 2004, June. Rao- Blackwellized Monte Carlo data association for multiple target tracking. In Proceedings of the seventh international conference on information fusion (Vol. 1, pp. 583-590). I.; [2] McCormack, L., Politis, A. Sa"rkka", S., and Pulkki, V., 2021. Real-Time Tracking of Multiple Acoustical Sources Utilising Rao-Blackwellised Particle Filtering. In 29th European Signal Processing Conference (EUSIPCO), (pp. 206-210).

Author: Leo McCormack

Date: 12.08.2020

License: GNU GPLv2

Definition in file saf_tracker_internal.c.

Macro Definition Documentation

◆ SAF_LOG_2PI

#define SAF_LOG_2PI ( logf(2.0f*SAF_PI) )

log(2pi)

Definition at line 50 of file saf_tracker_internal.c.

Function Documentation

◆ categ_rnd()

int categ_rnd	(	float *	P,
		int	len_P )

Draws samples from a given one dimensional discrete distribution.

Parameters

[in]	P	Discrete distribution; len_P x 1
[in]	len_P	length of P

Definition at line 908 of file saf_tracker_internal.c.

◆ eff_particles()

float eff_particles	(	voidPtr *	SS,
		int	NP )

Estimate the number of effective particles.

Warning: This function assumes that the weights have been normalised!

Parameters

[in]	SS	Array of particle structures; NP x 1
[in]	NP	Number of particle structures

Returns: Number of effective particles

Definition at line 560 of file saf_tracker_internal.c.

◆ gamma_cdf()

float gamma_cdf	(	float	x,
		float	gam,
		float	beta,
		float	mu )

Cumulative density function of a Gamma distribution.

Parameters

[in]	x	Locations where to evaluate the CDF
[in]	gam	Parameter of the distribution
[in]	beta	Parameter of the distribution
[in]	mu	Mean of the distribution

Returns: Cumulative density at the given locations

Definition at line 740 of file saf_tracker_internal.c.

◆ gauss_pdf3()

float gauss_pdf3	(	void *const	hUp6,
		float	X[3],
		float	M[3],
		float	S[3][3] )

Multivariate Gaussian PDF.

Calculate values of PDF (Probability Density Function) of multivariate Gaussian distribution: N(X | M, S)

Note: This has been hard-coded for N=3, but a general version should be quite straight-forward (just not needed yet)...

Parameters

[in]	hUp6	Handle for helper structure
[in]	X	Values
[in]	M	Mean of distibution or values as 3x3 matrix.
[in]	S	3x3 covariance matrix

Returns: Probability of X.

Definition at line 870 of file saf_tracker_internal.c.

◆ incompletegamma()

static double incompletegamma	(	double	a,
		double	x )

static

Incomplete gamma integral.

The function is defined by:

*                          x
*                           -
*                  1       | |  -t  a-1
* igam(a,x)  =   -----     |   e   t   dt.
*                 -      | |
*                | (a)    -
*                          0
*

In this implementation both arguments must be positive. The integral is evaluated by either a power series or continued fraction expansion, depending on the relative values of a and x.

Definition at line 1097 of file saf_tracker_internal.c.

◆ incompletegammac()

static double incompletegammac	(	double	a,
		double	x )

static

Complemented incomplete gamma integral.

The function is defined by

* igamc(a,x)   =   1 - igam(a,x)
*
*                           inf.
*                             -
*                    1       | |  -t  a-1
*              =   -----     |   e   t   dt.
*                   -      | |
*                  | (a)    -
*                            x
*

In this implementation both arguments must be positive. The integral is evaluated by either a power series or continued fraction expansion, depending on the relative values of a and x.

Definition at line 1033 of file saf_tracker_internal.c.

◆ kf_predict6()

void kf_predict6	(	float	M[6],
		float	P[6][6],
		float	A[6][6],
		float	Q[6][6] )

Perform Kalman Filter prediction step.

The model is: x[k] = A*x[k-1] + B*u[k-1] + q, q ~ N(0,Q). The predicted state is distributed as follows: p(x[k] | x[k-1]) = N(x[k] | A*x[k-1], Q[k-1])

The predicted mean x-[k] and covariance P-[k] are calculated with the following equations: m-[k] = A*x[k-1] P-[k] = A*P[k-1]*A' + Q.

Note: This has been hard-coded for N=6 and without 'B' and 'u', but a general version should be quite straight-forward (just not needed yet)...

Parameters

[in,out]	M	Mean state estimate of previous step
[in,out]	P	State covariance of previous step
[in]	A	Transition matrix of discrete model
[in]	Q	Process noise of discrete model

Definition at line 601 of file saf_tracker_internal.c.

◆ kf_update6()

void kf_update6	(	void *const	hUp6,
		float	X[6],
		float	P[6][6],
		float	y[3],
		float	H[3][6],
		float	R[3][3],
		float	X_out[6],
		float	P_out[6][6],
		float *	LH )

Kalman Filter update step.

Kalman Filter model is: x[k] = A*x[k-1] + B*u[k-1] + q, q ~ N(0,Q) y[k] = H*x[k] + r, r ~ N(0,R)

Prediction step of Kalman filter computes predicted mean m-[k] and covariance P-[k] of state: p(x[k] | y[1:k-1]) = N(x[k] | m-[k], P-[k])

See for instance kf_predict6() how m-[k] and P-[k] are calculated.

Update step computes the posterior mean m[k] and covariance P[k] of state given new measurement: p(x[k] | y[1:k]) = N(x[k] | m[k], P[k])

Innovation distribution is defined as: p(y[k] | y[1:k-1]) = N(y[k] | IM[k], IS[k])

Updated mean x[k] and covarience P[k] are given by the following equations (not the only possible ones): v[k] = y[k] - H[k]*m-[k] S[k] = H[k]*P-[k]*H[k]' + R[k] K[k] = P-[k]*H[k]'*[S[k]]^(-1) m[k] = m-[k] + K[k]*v[k] P[k] = P-[k] - K[k]*S[k]*K[k]'

Note: This has been hard-coded for N=6 and without 'K', 'IM' and 'IS', but a general version should be quite straight-forward (just not needed yet).

Parameters

[in]	hUp6	Handle for helper structure
[in]	X	Nx1 mean state estimate after prediction step
[in]	P	NxN state covariance after prediction step
[in]	y	Dx1 measurement vector.
[in]	H	Measurement matrix.
[in]	R	Measurement noise covariance.
[out]	X_out	Updated state mean
[out]	P_out	Updated state covariance
[out]	LH	(&) Predictive probability (likelihood) of measurement.

Definition at line 653 of file saf_tracker_internal.c.

◆ kf_update6_create()

void kf_update6_create ( void **const phUp6 )

Creates helper structure for kf_update6()

Definition at line 630 of file saf_tracker_internal.c.

◆ kf_update6_destroy()

void kf_update6_destroy ( void **const phUp6 )

Destroys helper structure for kf_update6()

Definition at line 638 of file saf_tracker_internal.c.

◆ lngamma()

static double lngamma	(	double	x,
		double *	sgngam )

static

Natural logarithm of gamma function.

Parameters

[in]	x	input
[in,out]	sgngam	(&) sign(Gamma(X))

Returns: logarithm of the absolute value of the Gamma(X).

Definition at line 939 of file saf_tracker_internal.c.

◆ lti_disc()

void lti_disc	(	float *	F,
		int	len_N,
		int	len_Q,
		float *	opt_L,
		float *	opt_Qc,
		float	dt,
		float *	A,
		float *	Q )

LTI_DISC Discretize LTI ODE with Gaussian Noise.

Discretize LTI ODE with Gaussian Noise. The original ODE model is in form: dx/dt = F x + L w, w ~ N(0,Qc)

Result of discretization is the model: x[k] = A x[k-1] + q, q ~ N(0,Q)

Which can be used for integrating the model exactly over time steps, which are multiples of dt.

Parameters

[in]	F	Square feedback matrix; FLAT: len_N x len_N
[in]	len_N	Size of square matrix 'F'
[in]	len_Q	Size of square matrix 'opt_Qc'
[in]	opt_L	Noise effect matrix (optional, set to NULL for default values); FLAT: len_N x len_Q
[in]	opt_Qc	Diagonal Spectral Density (optional, set to NULL for default values); FLAT: len_Q x len_Q
[in]	dt	Time Step
[out]	A	Transition matrix; FLAT: len_N x len_N
[out]	Q	Discrete Process Covariance; FLAT: len_N x len_N

Definition at line 755 of file saf_tracker_internal.c.

◆ normalise_weights()

void normalise_weights	(	voidPtr *	SS,
		int	NP )

Normalises the weights of the given particles.

Parameters

[in,out]	SS	Array of particle structures; NP x 1
[in]	NP	Number of particle structures

Definition at line 579 of file saf_tracker_internal.c.

◆ resampstr()

void resampstr	(	voidPtr *	SS,
		int	NP,
		int *	s )

Stratified resampling - returns a new set of indices according to the probabilities P.

Sorted re-sampling is slower but has slightly smaller variance. Stratified resampling is unbiased, almost as fast as deterministic resampling, and has only slightly larger variance.

In stratified resampling indices are sampled using random numbers [1] u_j~U[(j-1)/n,j/n], where n is length of P. Compare this to simple random resampling where u_j~U[0,1].

Warning: This function assumes that the weights have been normalised!

Parameters

[in]	SS	Array of particle structures; NP x 1
[in]	NP	Number of particle structures
[out]	s	Resampled indices; NP x 1

See also: [1] Kitagawa, G., Monte Carlo Filter and Smoother for Non-Gaussian Nonlinear State Space Models, Journal of Computational and Graphical Statistics, 5(1):1-25, 1996.

Definition at line 526 of file saf_tracker_internal.c.

◆ tracker3d_getMaxParticleIdx()

int tracker3d_getMaxParticleIdx ( void *const hT3d )

Returns the index of the most important particle.

Parameters

[in] hT3d tracker3d handle

Returns: index of the most important particle

Definition at line 499 of file saf_tracker_internal.c.

◆ tracker3d_particleCopy()

void tracker3d_particleCopy	(	void *	hPart1,
		void *	hPart2 )

Copies particle structure "hPart1" into structure "hPart2".

Parameters

[in]	hPart1	Particle structure 1
[in]	hPart2	Particle structure 2

Definition at line 165 of file saf_tracker_internal.c.

◆ tracker3d_particleCreate()

void tracker3d_particleCreate	(	void **	phPart,
		float	W0,
		float	dt )

Creates an instance of a particle / Monte-Carlo Sample.

Parameters

[in]	phPart	(&) address of particle structure
[in]	W0	Importance weight PRIOR
[in]	dt	Time step

Definition at line 128 of file saf_tracker_internal.c.

◆ tracker3d_particleDestroy()

void tracker3d_particleDestroy ( void ** phPart )

Destroys an instance of a particle / Monte-Carlo Sample.

Parameters

[in] phPart (&) address of particle structure

Definition at line 185 of file saf_tracker_internal.c.

◆ tracker3d_particleReset()

void tracker3d_particleReset ( void * hPart )

Resets a particle structure to defaults.

Parameters

[in] hPart Particle structure

Definition at line 149 of file saf_tracker_internal.c.

◆ tracker3d_predict()

void tracker3d_predict	(	void *const	hT3d,
		int	Tinc )

Prediction step.

Parameters

[in]	hT3d	tracker3d handle
[in]	Tinc	Number of time steps to increment by

Definition at line 202 of file saf_tracker_internal.c.

◆ tracker3d_update()

void tracker3d_update	(	void *const	hT3d,
		float *	Y,
		int	Tinc )

Prediction update.

Parameters

[in]	hT3d	tracker3d handle
[in]	Y	New observation/measurement; 3 x 1
[in]	Tinc	Number of time steps to increment by

Definition at line 357 of file saf_tracker_internal.c.

Data Structures

Macros

Functions

Detailed Description

Macro Definition Documentation

◆ SAF_LOG_2PI

Function Documentation

◆ categ_rnd()

◆ eff_particles()

◆ gamma_cdf()

◆ gauss_pdf3()

◆ incompletegamma()

◆ incompletegammac()

◆ kf_predict6()

◆ kf_update6()

◆ kf_update6_create()

◆ kf_update6_destroy()

◆ lngamma()

◆ lti_disc()

◆ normalise_weights()

◆ resampstr()

◆ tracker3d_getMaxParticleIdx()

◆ tracker3d_particleCopy()

◆ tracker3d_particleCreate()

◆ tracker3d_particleDestroy()

◆ tracker3d_particleReset()

◆ tracker3d_predict()

◆ tracker3d_update()