SAF
Loading...
Searching...
No Matches
saf_tracker.c
Go to the documentation of this file.
1/*
2 * This file is part of the saf_tracker module.
3 * Copyright (c) 2020 - Leo McCormack
4 *
5 * The saf_tracker module is free software; you can redistribute it and/or
6 * modify it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or (at your
8 * option) any later version.
9 *
10 * The saf_tracker module is distributed in the hope that it will be useful, but
11 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
12 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
13 * more details.
14 *
15 * See <http://www.gnu.org/licenses/> for a copy of the GNU General Public
16 * License.
17 */
18
44#include "saf_tracker_internal.h"
45#include "saf_tracker.h"
46
47#ifdef SAF_ENABLE_TRACKER_MODULE
48
49void tracker3d_create
50(
51 void ** const phT3d,
52 tracker3d_config tpars
53)
54{
55 tracker3d_data* pData = (tracker3d_data*)malloc1d(sizeof(tracker3d_data));
56 *phT3d = (void*)pData;
57 int i;
58 float sd_xyz, q_xyz;
59 float Qc[6][6];
60
61 /* Store user configuration */
62 pData->tpars = tpars;
63
64 /* Parameter checking */
65 pData->tpars.Np = SAF_CLAMP(pData->tpars.Np, 1, TRACKER3D_MAX_NUM_PARTICLES);
66 saf_assert(pData->tpars.ARE_UNIT_VECTORS == 0 || pData->tpars.ARE_UNIT_VECTORS == 1, "ARE_UNIT_VECTORS is a bool");
67 pData->tpars.init_birth = SAF_CLAMP(pData->tpars.init_birth, 0.0f, 0.99f);
68 pData->tpars.alpha_death = SAF_CLAMP(pData->tpars.alpha_death, 1.0f, 20.0f);
69 pData->tpars.beta_death = SAF_CLAMP(pData->tpars.beta_death, 1.0f, 20.0f);
70 pData->tpars.dt = SAF_MAX(pData->tpars.dt, 0.0001f);
71 pData->tpars.cd = SAF_MAX(pData->tpars.cd, 0.0001f);
72 pData->tpars.W_avg_coeff = SAF_CLAMP(pData->tpars.W_avg_coeff, 0.0f, 0.99f);
73 pData->tpars.noiseSpecDen = SAF_MAX(pData->tpars.noiseSpecDen, 0.0001f);
74 pData->tpars.noiseLikelihood = SAF_CLAMP(pData->tpars.noiseLikelihood, 0.0f, 0.99f);
75 pData->tpars.measNoiseSD = SAF_MAX(pData->tpars.measNoiseSD, 0.001f);
76
77 /* Measurement noise PRIORs along the x,y,z axis, respectively */
78 sd_xyz = pData->tpars.measNoiseSD;
79 memset(pData->R, 0, 3*3*sizeof(float));
80 pData->R[0][0] = powf(sd_xyz,2.0f);
81 pData->R[1][1] = powf(sd_xyz,2.0f);
82 pData->R[2][2] = powf(sd_xyz,2.0f);
83
84 /* Noise spectral density along x, y, z axis qx,y,z which, (in combination
85 * with sd_xyz), dictates how smooth the target tracks are. */
86 q_xyz = pData->tpars.noiseSpecDen;
87
88 /* Dynamic and measurement models */
89 const float F[6][6] =
90 { {0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f},
91 {0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f},
92 {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f},
93 {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
94 {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
95 {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f} };
96 memset(Qc, 0, 6*6*sizeof(float));
97 Qc[3][3] = q_xyz;
98 Qc[4][4] = q_xyz;
99 Qc[5][5] = q_xyz;
100 lti_disc((float*)F, 6, 6, NULL, (float*)Qc, pData->tpars.dt, (float*)pData->A, (float*)pData->Q);
101 memset(pData->H, 0, 3*6*sizeof(float));
102 pData->H[0][0] = 1.0f;
103 pData->H[1][1] = 1.0f;
104 pData->H[2][2] = 1.0f;
105
106 /* Create kalman filter helper struct */
107 kf_update6_create(&(pData->hKF6));
108
109 /* Create particles */
110 pData->SS = malloc1d(pData->tpars.Np * sizeof(voidPtr));
111 pData->SS_resamp = malloc1d(pData->tpars.Np * sizeof(voidPtr));
112 pData->W0 = 1.0f/(float)pData->tpars.Np;
113 for(i=0; i<pData->tpars.Np; i++){
114 tracker3d_particleCreate(&(pData->SS[i]), pData->W0, pData->tpars.dt);
115 tracker3d_particleCreate(&(pData->SS_resamp[i]), pData->W0, pData->tpars.dt);
116 }
117
118 /* Event starting values */
119 for(i=0; i<TRACKER3D_MAX_NUM_EVENTS; i++){
120 pData->evta[i] = -1;
121 tracker3d_particleCreate(&(pData->str[i]), pData->W0, pData->tpars.dt);
122 }
123 pData->incrementTime = 0;
124}
125
126void tracker3d_destroy
127(
128 void ** const phT3d
129)
130{
131 tracker3d_data *pData = (tracker3d_data*)(*phT3d);
132 int i;
133
134 if (pData != NULL) {
135
136 kf_update6_destroy(&(pData->hKF6));
137
138 for(i=0; i<pData->tpars.Np; i++){
139 tracker3d_particleDestroy(&pData->SS[i]);
140 tracker3d_particleDestroy(&pData->SS_resamp[i]);
141 }
142 free(pData->SS);
143 free(pData->SS_resamp);
144
145 for(i=0; i<TRACKER3D_MAX_NUM_EVENTS; i++)
146 tracker3d_particleDestroy(&pData->str[i]);
147
148 free(pData);
149 pData = NULL;
150 *phT3d = NULL;
151 }
152}
153
154void tracker3d_reset
155(
156 void* const hT3d
157)
158{
159 tracker3d_data *pData = (tracker3d_data*)(hT3d);
160 int i;
161
162 pData->incrementTime = 0;
163 for(i=0; i<pData->tpars.Np; i++)
164 tracker3d_particleReset(pData->SS[i]);
165}
166
167void tracker3d_step
168(
169 void* const hT3d,
170 float* newObs_xyz,
171 int nObs,
172 float** target_pos_xyz,
173 float** target_var_xyz,
174 int** target_IDs,
175 int* nTargets
176)
177{
178 tracker3d_data *pData = (tracker3d_data*)(hT3d);
179 int i, kt, ob, maxIdx, nt;
180 float Neff;
181 int s[TRACKER3D_MAX_NUM_PARTICLES];
182 MCS_data* S_max;
183#if 0
184 int nt2;
185 float w_sum;
186 MCS_data* S_tmp;
187#endif
188#ifdef TRACKER_VERBOSE
189 char c_str[256], tmp[256];
190 memset(c_str, 0, 256*sizeof(char));
191#endif
192
193 pData->incrementTime++;
194
195 /* Loop over measurements */
196 if(nObs>0 && newObs_xyz!=NULL){
197 for(ob=0; ob<nObs; ob++){
198 /* Predict and update steps */
199 for (kt = 0; kt < pData->incrementTime; kt++)
200 tracker3d_predict(hT3d, 1);
201 tracker3d_update(hT3d, &newObs_xyz[ob*3], pData->incrementTime);
202 pData->incrementTime = 0;
203
204 /* Resample if needed */
205 Neff = eff_particles(pData->SS, pData->tpars.Np);
206 if (Neff < (float)pData->tpars.Np/4.0f){
207#ifdef TRACKER_VERBOSE
208 printf("%s\n", "Resampling");
209#endif
210 maxIdx = tracker3d_getMaxParticleIdx(hT3d);
211 for(i=0; i<pData->tpars.Np; i++)
212 s[i] = maxIdx;
213 //resampstr(pData->SS, pData->tpars.Np, s);
214
215 for(i=0; i<pData->tpars.Np; i++)
216 tracker3d_particleCopy(pData->SS[s[i]], pData->SS_resamp[i]);
217 for(i=0; i<pData->tpars.Np; i++){
218 tracker3d_particleCopy(pData->SS_resamp[i], pData->SS[i]);
219 ((MCS_data*)pData->SS[i])->W = ((MCS_data*)pData->SS[i])->W0;
220 }
221 }
222
223 /* Apply (optional) temporal smoothing of the particle importance weights */
224 if(pData->tpars.W_avg_coeff>0.0001f){
225 for(i=0; i<pData->tpars.Np; i++){
226 ((MCS_data*)pData->SS[i])->W = ((MCS_data*)pData->SS[i])->W * (1.0f-pData->tpars.W_avg_coeff) +
227 ((MCS_data*)pData->SS[i])->W_prev * pData->tpars.W_avg_coeff;
228 ((MCS_data*)pData->SS[i])->W_prev = ((MCS_data*)pData->SS[i])->W;
229 }
230 }
231 }
232 }
233
234 /* Find most dominant particle.. */
235 maxIdx = tracker3d_getMaxParticleIdx(hT3d);
236 S_max = (MCS_data*)pData->SS[maxIdx];
237
238 /* Output */
239 if(S_max->nTargets==0){
240 free((*target_pos_xyz));
241 free((*target_var_xyz));
242 free((*target_IDs));
243 (*target_pos_xyz) = NULL;
244 (*target_var_xyz) = NULL;
245 (*target_IDs) = NULL;
246 (*nTargets) = 0;
247#ifdef TRACKER_VERBOSE
248 printf("%s\n", "No targets");
249#endif
250 }
251 else{
252 (*target_pos_xyz) = realloc1d((*target_pos_xyz), S_max->nTargets*3*sizeof(float));
253 (*target_var_xyz) = realloc1d((*target_var_xyz), S_max->nTargets*3*sizeof(float));
254 (*target_IDs) = realloc1d((*target_IDs), S_max->nTargets*sizeof(int));
255 (*nTargets) = S_max->nTargets;
256
257 /* Loop over targets */
258 for(nt=0; nt<S_max->nTargets; nt++){
259#ifdef TRACKER_VERBOSE
260 sprintf(tmp, "ID_%d: [%.5f,%.5f,%.5f] ", S_max->targetIDs[nt], S_max->M[nt].m0, S_max->M[nt].m1, S_max->M[nt].m2);
261 strcat(c_str, tmp);
262#endif
263 /* Target IDs are based on those defined by the most dominant particle */
264 (*target_IDs)[nt] = S_max->targetIDs[nt];
265 (*target_pos_xyz)[nt*3] = S_max->M[nt].m0;
266 (*target_pos_xyz)[nt*3+1] = S_max->M[nt].m1;
267 (*target_pos_xyz)[nt*3+2] = S_max->M[nt].m2;
268 (*target_var_xyz)[nt*3] = S_max->P[nt].p00;
269 (*target_var_xyz)[nt*3+1] = S_max->P[nt].p11;
270 (*target_var_xyz)[nt*3+2] = S_max->P[nt].p22;
271
272# if 0
273 /* Apply the corresponding importance weight */
274 w_sum = S_max->W;
275 (*target_pos_xyz)[nt*3] *= S_max->W;
276 (*target_pos_xyz)[nt*3+1] *= S_max->W;
277 (*target_pos_xyz)[nt*3+2] *= S_max->W;
278 (*target_var_xyz)[nt*3] *= S_max->W;
279 (*target_var_xyz)[nt*3+1] *= S_max->W;
280 (*target_var_xyz)[nt*3+2] *= S_max->W;
281
282 /* Loop over all of the other particles - importance sampling */
283 for(int p = 0; p<pData->tpars.Np; p++){
284 if(p!=maxIdx){
285 S_tmp = (MCS_data*)pData->SS[p];
286 for(nt2=0; nt2<S_tmp->nTargets; nt2++){
287 if((*target_IDs)[nt] == S_tmp->targetIDs[nt2]){
288 w_sum += S_tmp->W;
289 (*target_pos_xyz)[nt*3] += (S_tmp->M[nt2].m0 * S_tmp->W);
290 (*target_pos_xyz)[nt*3+1] += (S_tmp->M[nt2].m1 * S_tmp->W);
291 (*target_pos_xyz)[nt*3+2] += (S_tmp->M[nt2].m2 * S_tmp->W);
292 (*target_var_xyz)[nt*3] += (S_tmp->P[nt2].p00 * S_tmp->W);
293 (*target_var_xyz)[nt*3+1] += (S_tmp->P[nt2].p11 * S_tmp->W);
294 (*target_var_xyz)[nt*3+2] += (S_tmp->P[nt2].p22 * S_tmp->W);
295 }
296 }
297 }
298 }
299
300 /* Renormalise based on the combined importance weights */
301 (*target_pos_xyz)[nt*3] /= w_sum;
302 (*target_pos_xyz)[nt*3+1] /= w_sum;
303 (*target_pos_xyz)[nt*3+2] /= w_sum;
304 (*target_var_xyz)[nt*3] /= w_sum;
305 (*target_var_xyz)[nt*3+1] /= w_sum;
306 (*target_var_xyz)[nt*3+2] /= w_sum;
307#endif
308 }
309#ifdef TRACKER_VERBOSE
310 printf("%s\n", c_str);
311#endif
312 }
313}
314
315#endif /* SAF_ENABLE_TRACKER_MODULE */
tracker3d_reset
void tracker3d_reset(void *const hT3d)
Resets an instance of the mighty tracker3d.
Definition saf_tracker.c:155
tracker3d_destroy
void tracker3d_destroy(void **const phT3d)
Destroys an instance of the mighty tracker3d.
Definition saf_tracker.c:127
tracker3d_step
void tracker3d_step(void *const hT3d, float *newObs_xyz, int nObs, float **target_pos_xyz, float **target_var_xyz, int **target_IDs, int *nTargets)
Tracker time step to update & predict current target locations and to parse new measurements/observat...
Definition saf_tracker.c:168
tracker3d_create
void tracker3d_create(void **const phT3d, tracker3d_config tpars)
Creates an instance of the mighty tracker3d.
Definition saf_tracker.c:50
saf_assert
#define saf_assert(x, message)
Macro to make an assertion, along with a string explaining its purpose.
Definition saf_utilities.h:133
SAF_CLAMP
#define SAF_CLAMP(a, min, max)
Ensures value "a" is clamped between the "min" and "max" values.
Definition saf_utilities.h:61
SAF_MAX
#define SAF_MAX(a, b)
Returns the maximum of the two values.
Definition saf_utilities.h:58
malloc1d
void * malloc1d(size_t dim1_data_size)
1-D malloc (same as malloc, but with error checking)
Definition md_malloc.c:59
realloc1d
void * realloc1d(void *ptr, size_t dim1_data_size)
1-D realloc (same as realloc, but with error checking)
Definition md_malloc.c:79
voidPtr
void * voidPtr
Void pointer (just to improve code readability when working with arrays of handles)
Definition saf_reverb_internal.h:72
saf_tracker.h
Particle filtering based 3D multi-target tracker (SAF_TRACKER_MODULE)
tracker3d_update
void tracker3d_update(void *const hT3d, float *Y, int Tinc)
Prediction update.
Definition saf_tracker_internal.c:358
tracker3d_particleDestroy
void tracker3d_particleDestroy(void **phPart)
Destroys an instance of a particle / Monte-Carlo Sample.
Definition saf_tracker_internal.c:186
tracker3d_particleReset
void tracker3d_particleReset(void *hPart)
Resets a particle structure to defaults.
Definition saf_tracker_internal.c:150
kf_update6_create
void kf_update6_create(void **const phUp6)
Creates helper structure for kf_update6()
Definition saf_tracker_internal.c:630
tracker3d_particleCopy
void tracker3d_particleCopy(void *hPart1, void *hPart2)
Copies particle structure "hPart1" into structure "hPart2".
Definition saf_tracker_internal.c:166
tracker3d_predict
void tracker3d_predict(void *const hT3d, int Tinc)
Prediction step.
Definition saf_tracker_internal.c:203
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.
Definition saf_tracker_internal.c:756
tracker3d_getMaxParticleIdx
int tracker3d_getMaxParticleIdx(void *const hT3d)
Returns the index of the most important particle.
Definition saf_tracker_internal.c:500
tracker3d_particleCreate
void tracker3d_particleCreate(void **phPart, float W0, float dt)
Creates an instance of a particle / Monte-Carlo Sample.
Definition saf_tracker_internal.c:129
eff_particles
float eff_particles(voidPtr *SS, int NP)
Estimate the number of effective particles.
Definition saf_tracker_internal.c:561
kf_update6_destroy
void kf_update6_destroy(void **const phUp6)
Destroys helper structure for kf_update6()
Definition saf_tracker_internal.c:638
saf_tracker_internal.h
Particle filtering based 3D multi-target tracker (SAF_TRACKER_MODULE)
TRACKER3D_MAX_NUM_EVENTS
#define TRACKER3D_MAX_NUM_EVENTS
Maximum number of possible events during update.
Definition saf_tracker_internal.h:67
TRACKER3D_MAX_NUM_PARTICLES
#define TRACKER3D_MAX_NUM_PARTICLES
Maximum number of particles.
Definition saf_tracker_internal.h:69
MCS_data
Monte-Carlo Sample (particle) structure.
Definition saf_tracker_internal.h:103
MCS_data::P
P66 * P
Current target variances; nTargets x ([6][6])
Definition saf_tracker_internal.h:110
MCS_data::W
float W
Importance weight.
Definition saf_tracker_internal.h:104
MCS_data::targetIDs
int * targetIDs
Unique ID assigned to each target; nTargets x 1.
Definition saf_tracker_internal.h:111
MCS_data::nTargets
int nTargets
Number of targets being tracked.
Definition saf_tracker_internal.h:107
MCS_data::M
M6 * M
Current target means; nTargets x ([6])
Definition saf_tracker_internal.h:109
tracker3d_config
User parameters for tracker3d.
Definition saf_tracker.h:59
tracker3d_config::dt
float dt
Elapsed time (in seconds) between observations/measurements.
Definition saf_tracker.h:84
tracker3d_config::cd
float cd
PRIOR probability of noise.
Definition saf_tracker.h:106
tracker3d_config::init_birth
float init_birth
PRIOR probability of birth [0 1].
Definition saf_tracker.h:79
tracker3d_config::measNoiseSD
float measNoiseSD
Measurement noise standard deviation.
Definition saf_tracker.h:71
tracker3d_config::ARE_UNIT_VECTORS
int ARE_UNIT_VECTORS
1: if the Cartesian coordinates are given as unit vectors, 0: if not
Definition saf_tracker.h:65
tracker3d_config::beta_death
float beta_death
Coefficient influencing the likelihood that a target will die; always >= 1.
Definition saf_tracker.h:82
tracker3d_config::alpha_death
float alpha_death
Coefficient influencing the likelihood that a target will die; always >= 1.
Definition saf_tracker.h:80
tracker3d_config::Np
int Np
Number of Monte Carlo samples/particles.
Definition saf_tracker.h:60
tracker3d_config::noiseLikelihood
float noiseLikelihood
Likelihood of an estimate being noise/clutter between [0..1].
Definition saf_tracker.h:69
tracker3d_config::W_avg_coeff
float W_avg_coeff
Real-time tracking is based on the particle with highest weight.
Definition saf_tracker.h:86
tracker3d_config::noiseSpecDen
float noiseSpecDen
Noise spectral density; influences the smoothness of the traget tracks.
Definition saf_tracker.h:74
tracker3d_data
Main structure for tracker3d.
Definition saf_tracker_internal.h:122
tracker3d_data::incrementTime
int incrementTime
Number steps of "tpars.dt" to increment time by.
Definition saf_tracker_internal.h:135
tracker3d_data::W0
float W0
PRIOR importance weight.
Definition saf_tracker_internal.h:136
tracker3d_data::Q
float Q[6][6]
Discrete Process Covariance.
Definition saf_tracker_internal.h:133
tracker3d_data::tpars
tracker3d_config tpars
User parameters struct.
Definition saf_tracker_internal.h:124
tracker3d_data::R
float R[3][3]
Diagonal matrix, measurement noise PRIORs along the x,y,z axes.
Definition saf_tracker_internal.h:130
tracker3d_data::evta
int evta[TRACKER3D_MAX_NUM_EVENTS]
Event targets.
Definition saf_tracker_internal.h:142
tracker3d_data::SS_resamp
voidPtr * SS_resamp
Resampled particles; tpars.Np x 1.
Definition saf_tracker_internal.h:129
tracker3d_data::str
voidPtr str[TRACKER3D_MAX_NUM_EVENTS]
Structure after each event.
Definition saf_tracker_internal.h:146
tracker3d_data::SS
voidPtr * SS
The particles; tpars.Np x 1.
Definition saf_tracker_internal.h:128
tracker3d_data::A
float A[6][6]
Transition matrix.
Definition saf_tracker_internal.h:132
tracker3d_data::H
float H[3][6]
Measurement matrix.
Definition saf_tracker_internal.h:134
tracker3d_data::hKF6
void * hKF6
kf_update6 handle
Definition saf_tracker_internal.h:127