Tracking position and velocity using a kalman filter

6
votes

I am using a kalman filter (constant velocity model) to track postion and velocity of an object. I measure x,y of the object and track x,y,vx,vy . Which works but if a add gausian noise of +- 20 mm to the sensor readings x,y,vx,vy fluctuates even though the point is not moving just noise. For location that is good enough for my needs but velocity changes when the point is stationary and that is causing problems with my object speed calculations. Is there a way around this problem? also if switching to constant acceleration model improve on this? I am tracking a robot via a camera.

I am using opencv implementation and my kalman model is same as [1]

[1] http://www.morethantechnical.com/2011/06/17/simple-kalman-filter-for-tracking-using-opencv-2-2-w-code/

1
opencvlocalizationroboticskalman-filter

1 Answers

3
votes

The most important thing about designing a Kalman filter is not the data, it's the error estimates. The matrices in that example seem to be chosen arbitrarily, but you should pick them using specific knowledge of your system. In particular:

  • Error covariance has units. It's standard deviation squared. So your position error is in "length squared" and velocity in "length per time squared". The values in those matrices will be different depending on whether you work in m or mm.
  • You are implementing a "constant velocity" model, but the "processNoiseCov" from the example sets the same values for both position and velocity process noise. This means that you might be wrong about your position due to being wrong about your velocity, and you might be wrong because the object teleported around in a way that's independent of velocity. In a CV model you would expect that the position process noise would be very low (basically nonzero only for numerical reasons and to cover modelling error) and the true unknown motion of the system would be attributed to unknown velocity. This problem also interferes with the KF's ability to infer velocity from position input, because the "teleportation error" of position is not attributed to velocity change.
  • If you're putting in +/-20mm of error (you should really put in Gaussian noise if you want to simulate ideal behavior) you have an approximate standard deviation of 11.5mm or a variance of (11.5mm)^2. Not knowing what your units are (mm or m) I can't say what the numerical value of "measurementNoiseCov" should be, but it's not 0.1 (as in the example).

And finally, even with all of that correct, keep in mind that the KF is ultimately a linear filter. Whatever noise you put in will show up in the output, just scaled by some factor (the Kalman gain).