This is an implementation of the Gaussian mixture probability hypothesis density filter (GM-PHD) described in:
B.-N. Vo, W.-K. Ma, "The Gaussian Mixture Probability Hypothesis Density Filter", IEEE Transactions on Signal Processing, Vol 54, No. 11, November 2006, pp4091-4104.
The submission includes a Matlab implementation of the linear Kalman filter GM-PHD filter and the extended Kalman filter GM-PHD filter algorithms described by Vo & Ma, as well as one of the simulated problems described in their paper. A few modifications were made from Vo & Ma's algorithm but they are for technical reasons and don't change the overall structure of the filter.
The GM-PHD filter is a means of estimating the number and positions of targets in measurement data. Its advantages include a representation of target position uncertainty (using a covariance matrix) as well as target existence uncertainty (using a weight) and the absence of data association in the update step. This implementation is quite heavily commented and will probably be helpful to people trying to learn about GM-PHD filtering, but the paper by Vo & Ma is essential to understand what is really going on.
The simulator creates noisy measurements of two moving targets in a cluttered environment, with a third target being created approximately halfway through the simulation. It's a fairly simple problem but it is effective in demonstrating filter performance. Vo & Ma use it to demonstrate the linear GM-PHD filter but I use it for the EKF version as well, rather than coding up the EKF simulator that they describe.
Ba-Ngu Vo has kindly allowed me to include his implementation of the Optimal Subpattern Assignment (OSPA) metric proposed by Schuhmacher et al in:
Schuhmacher, D.; Ba-Tuong Vo; Ba-Ngu Vo, "A Consistent Metric for Performance Evaluation of Multi-Object Filters," Signal Processing, IEEE Transactions on , vol.56, no.8, pp.3447,3457, Aug. 2008.
This isn't necessary for the GM-PHD filter to work but provides a nice way of visualising and quantifying performance. If people want to tweak this code, the metric is a nice way to see how well things are performing and it is fairly easy to modify this implementation to perform multiple test runs with the same data but different filter parameters and compare the metrics.
Read the README.txt, or just start with GM_PHD_Filter.m and work from there.
Could you please email me (firstname.lastname@example.org) the missing m file below:
Quang M Lam
To my understanding, new targets can only "birth" (assume no spawning) at predetermined positions defined in the birth intensity model.
As a result, how PHD can pick up the new targets "birth" at time step other than t=0. Assume measurements for these new targets are available.
Thanks in advance.
Just a very minor detail. As per MATLAB R2013b deg2rad (used in GM_EKF_PHD_Initialise_Jacobians) has been replaced by degtorad.
I am new to RFS and PDH filter. Could you please explain to me in the "GM_PHD_Initialisation.m" L147 and L147 about the target birth and spawn:
1) what does the weight "0.1 and 0.1" means in target birth.
2) what does the weight "0.05" means in target spawn.
Thanks a lot.
Hi, Bryan. I hava a question with the target modelling. In function `GM_PHD_Initialisation`, the standard deviation of process noise `sigma_v` is initialised. While in function `GM_PHD_Simulate_Measurements`, the functions for target 1, 2 and 3 are like `simTargetiState = F * simTarget1State`. It seems that the process noise has not been involved. And in function `GM_PHD_Predict_Birth`, there is also no process noise. Can you please tell me how did you treat with the process noise? Thank you.
Good find, it was used in the moving-target spawn of GM_PHD_Create_Birth at line 65 but was missed at the static-target spawn at line 116. It should be fixed now.
I'm currently implementing a GM-CPHD Filter, and - just to find out the similarities - I've read some old papers about the standard GM-PHD Filter, e.g., 'The Gaussian Mixture Probability Hypothesis Density Filter', Vo et al., 2006 (as mentioned in your description). Additionally, I've compared your GM-PHD implementation with mine. I've realized that you do not consider the old target weights when you calculate the spawning probabilities (see eq. 28 and tab. 1, step 1, 2nd loop in Vo's paper).
It should be
w_spawn = w_beta * w_old * N(·)
w_spawn = w_beta * N(·).
Please, let me know if I'm wrong. Thanks!
That was an untidy bug that I left in there because I kept forgetting to fix it. It didn't break anything, it just meant that the simTarget3Vel was half what it should have been. Well spotted, it should be gone now.
Hi Bryan. Thanks for your sharing code. I think it's great! At the same time, I have a question in function `GM_PHD_Simulate_Measurements`. Why does `simTarget3State = F * simTarget3State` been executed twice after the `simTarget3SpawnTime` during one iteration? At the first time, it was executed because `simTarget3State` will not be empty(since it will be constructed at the `simTarget3SpawnTime`) after the `simTarget3SpawnTime` at line 23-25, then it will be executed again since `k > simTarget3SpawnTime` at line 31-33. Can you please explain to me if I'm missing something? Thank you.
Good finds! They are fixed now, thank you for pointing them out.
I think I've discovered a small bug in your merging/pruning code. Let's check out the code first:
for i = 1:length(I)
thisI = I(i);
P_range = calculateDataRange4(i);
delta_m = m_k(:,thisI) - m_k(:,j);
mahal_dist = delta_m' * inv(P_k(:,P_range)) * delta_m;
if(mahal_dist <= mergeThresholdU)
L = [L, thisI];
The 'inv(P_k(:,P_range))' for distance computation causes some troubles: first, 'inv' isn't an accurate matrix-inversion operation, use 'mldivide' or '\' instead; second, and that's the main reason of this message, you use the correct 'm_k(:,thisI)' for your distance computation, but not the correct covariance-matrices 'P_k(:,P_range)'. You always (!) take the matrices with column indices '1:4','5:8',etc., instead of column indices corresponding to 'I(i)'. Please, let me know if I'm wrong.
Thanks again, I will try my best to digest the program. Best wishes.
Hi Ying. Currently the sensor is simulated as a direct measurement of target position in cartesian space; the measurement vector Z = [X; Y] where X = [x1, x2, ... xN], Y = [y1, y2, ... yN] for detected targets/clutter 1:N. See the file GM_PHD_Simulate_Measurements.m for how this is done. This is in line with the simulation described in Vo & Ma, and allows a linear Kalman filter to be used.
The same paper describes an extended Kalman filter EKF-PHD algorithm which can be used in nonlinear problems, such as those using range-bearing sensors. I will upload an implementation of this as a separate submission in a couple of days.
Hi, I want to know how did you simulate the radar sensor in the program.
Fixed a bug where, for some spawning settings, the weight of the existing target was not used to scale the weight of a target it spawned.
Removed CalculateOSPAMetric.m, added Ba-Ngu Vo's implementation ospa_dist.m which uses Alex Melin's Hungarian.m
Removed an error that was passing the OSPA metric the wrong data to calculate with. Fixed a bug that meant costs were not being calculated in the OSPA for measurements in Y unmatched in X. The results are much better now but still a bit buggy.
Modified GM_PHD_Create_Birth and GM_EKF_PHD_Create_Birth so that static targets are spawned from iteration 1 rather than waiting until iteration 2.
Fixed a small bug in a print statement.
I have included the extended Kalman filter algorithm described by Vo & Ma (the filter can be run as either linear KF or EKF). I also included a way of drawing velocity arrows on the main plot to demonstrate estimated target velocities.
A few bug fixes. A few new comments. Added a couple of features to better fit the Vo & Ma implementation. Added an implementation of the Optimal Subpattern Assignment (OSPA) metric proposed by Schuhmacher et al to quantify filter performance.
Fixed a bug in GM_PHD_Prune. Changed inv to \ in pruning and constructing update components.
Enhanced spawning for better compliance with Vo & Ma paper. Fixed error in spawn weight calculation. Minor plotting enhancements.
Added a few comments to hopefully make it clearer how to modify this code for different applications. Also did a bit of variable/renaming deleting but nothing major.
New targets can now be initialised with a 'spawned' weight rather than a 'birthed' weight. Whichever weight is higher is the one that is used. Previously all targets were birthed.
Fixed typos in the description.