Tracking multiple moving objects with KalmanFilter in OpenCV/C++ - How to assign tracks individually to detected objects

I am working on realtime movement detection and object tracking in OpenCV/C++ and am currently stuck at the tracking part.

Matlab-Example of what I want to do: http://www.mathworks.de/de/help/vision/examples/motion-based-multiple-object-tracking.html (I'm troubled with the tracking part and how to transfer it to C++/OpenCV)

My motion part works with OpenCVs BackgroundSubtractor MOG2 which makes it possible to find contours and filter smaller contours out.

For tracking I am currently trying to use the KalmanFilter (with a similar implementation to this) which right now is getting called in every frame if a moving object was found and draws a line on it's path. My Detection & Tracking part looks something like this:

BackgroundSubtractorMOG2 bg;
bg.operator()(frame, threshold);
bg.getBackgroundImage(background);
...  //morphological operations to remove noise etc.
findContours(threshold, ...);
...  //filtering to reject contours which are too smalle/too big

for(int i = 0; i < contours.size(); i++){
approxPolyDP(...);
boundRect = boundingRect(...);
x = boundRect.x + boundRect.width/2;
y = boundRect.y + boundRect.height/2;}

kalmanFilter.track(x,y);
kalmanFilter.draw(frame);

Current Problem: I have a scene with 0 moving objects, then 1 object moves in, gets detected through contours and gets tracked. Then a 2nd object moves in sight, gets detected and makes the tracker jump to it instead of following the first or marking both individually (which I want).

The current tracker takes x & y coordinates of the found object. Like this, once another object gets detected, the tracker still assumes it is the same object but with other coordinates than anticipated.

As one can see, there's no function to assign "tracks" to a certain object which is probably the biggest problem. I read about the hungarian algorithm but am not quite sure on how to implement it within my functions.

What would be a good method to make the tracking work for multiple objects?

My idea was that if I had each object uniquely identified I could check if the ID is still the same and if not let the tracker know that it's a new object to track separately from the other. Not sure if this is necessary or even useful though and if so, how to do it.

try something like this:

for each contour:
    if its already tracked with ID 'A': kalmanFilter_with_id_A.track(x,y);
    else createNewKalmanFilterWithID A

you need some mechanism to decide whether it is already tracked. In simple tracking you just decide by measuring distances to contours in last frame and if its close enough, it's the old object. This is quite erroneous so you might want to have a look at better tracking methods, e.g. probabilistic tracking.

so simple mode:

for each contour 'currentFrameC':
    for each contour 'lastFrameC'
        if distance(currentFrameC, lastFrameC) is smallest and < threshold
            currentFrameC is the same object as lastFrameC so give it the same ID
    if no shortest contour with dist < thres was found, create a new object with a new ID and create a new KalmanFilter with this same ID for that object

call kalmanFilter with ID for each found contour ID