saf_tracker_internal.h File Reference

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

#include "saf_tracker.h"
#include "../../modules/saf_utilities/saf_utilities.h"
#include "saf_externals.h"

Go to the source code of this file.

Data Structures
struct	M6
	Union struct for 3-D mean values. More...
struct	P66
	Union struct for 3-D variance values. More...
struct	MCS_data
	Monte-Carlo Sample (particle) structure. More...
struct	tracker3d_data
	Main structure for tracker3d. More...

Macros
#define	TRACKER3D_MAX_NUM_TARGETS   ( 24 )
	Spits out tracker status info to the terminal.
#define	TRACKER3D_MAX_NUM_EVENTS   ( 24 )
	Maximum number of possible events during update.
#define	TRACKER3D_MAX_NUM_PARTICLES   ( 100 )
	Maximum number of particles.

Typedefs
typedef void *	voidPtr
	Void pointer (just to improve code readability when working with arrays of handles)

Functions
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)

Based on the RBMCDA [1] MATLAB toolbox (GPLv2 license) by Simo Sa"rkka" and Jouni Hartikainen (Copyright (C) 2003-2008): https://users.aalto.fi/~ssarkka/#softaudio

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.h.

Macro Definition Documentation

TRACKER3D_MAX_NUM_EVENTS

#define TRACKER3D_MAX_NUM_EVENTS   ( 24 )

Maximum number of possible events during update.

Definition at line 67 of file saf_tracker_internal.h.

TRACKER3D_MAX_NUM_PARTICLES

#define TRACKER3D_MAX_NUM_PARTICLES   ( 100 )

Maximum number of particles.

Definition at line 69 of file saf_tracker_internal.h.

TRACKER3D_MAX_NUM_TARGETS

#define TRACKER3D_MAX_NUM_TARGETS   ( 24 )

Spits out tracker status info to the terminal.

Maximum number of targets that can be tracked

Definition at line 65 of file saf_tracker_internal.h.

Typedef Documentation

voidPtr

typedef void* voidPtr

Void pointer (just to improve code readability when working with arrays of handles)

Definition at line 77 of file saf_tracker_internal.h.

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

Original Copyright (C) 2002 Simo Särkkä, 2008 Jouni Hartikainen (GPLv2)

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

Original Copyright (C) 2003 Simo Särkkä, 2008 Jouni Hartikainen (GPLv2)

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

Original Copyright (C) 2003 Simo Särkkä, 2008 Jouni Hartikainen (GPLv2)

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.

Original Copyright (C) 2002 Simo Särkkä (GPLv2)

Definition at line 870 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

Original Copyright (C) 2002-2006 Simo Särkkä, 2007 Jouni Hartikainen (GPLv2)

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.

Original Copyright (C) 2002, 2003 Simo Särkkä, 2007 Jouni Hartikainen (GPLv2)

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.

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

Original Copyright (C) 2002, 2003 Simo Särkkä (GPLv2)

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

Original Copyright (C) 2008 Jouni Hartikainen (GPLv2)

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.

Original Copyright (c) 2003-2004 Aki Vehtari (GPLv2)

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.