Implementing Data Fusion via (Extended) Kalman-Filter in OpenCV / C ++
I am working on a project to track the position of a camera that is mounted on a moving device through data fusion. I receive data
1) speed in the x-, y- and z direction of the camera from source A
2) difference between positions in the current and last frame (in 2D, Z shouldn't change anyway) from source B
I have already done a similar project, but without any data merges or so, and used Kalman-Filter which is implemented in OpenCV.
I am currently confused all the time with all the different implementation methods I have found on the internet so far.
How can I insert / merge data I received in different KF / EKF components?
I have this example for modifying OpenCV-KF to work like EKF. It is very similar to what I need, except that my object is actually the camera itself and is also able to move along the y and x axis, in turn .... Also, instead of the pixel coordinates of the object, I got the above data. https://sites.google.com/site/timecontroll/home/extended-kalman-filtering-with-opencv
Unfortunately, I'm not really sure how to define my transition and measurement function and the corresponding matrix.
Let x (k) be the current state, deltaT is the elapsed time since the last update.
For 1 it will probably look something like this:
x (k) = x (k-1) + deltaT multiplied by rate (k-1)
For 2:
x (k) = x (k-1) + difference between frames
So, suppose both data sources are weighted the same, which makes me think that when combined, it would be something like:
x (k) = x (k-1) + (delta T multiplied by rate (k-1) + difference between frames) / 2
For measurements: I get the velocity in X, Y and Z directions through source A and the difference between current and the last frame in X and Y coordinates through source B.
Does this make sense, or am I already wrong about anything? How should I set up my transition and dimensions matrix and also update the current state accordingly?
source to share
You are on the right track, but you seem to be confused by the concepts of transition function and measurement function.
The first thing to do is to clearly define which state vector you want to consider. In your case, enough space [px, py, pz] and speed [vx, vy, vz]:
x k= [px k, py k, pz k, vx k, vy k, vz k]
The second task is to select a transition function to simulate the dynamics of your system. Note that this has nothing to do with sensor measurements: it is just a model of how the state of your system evolves over time. In the case of position and speed, a very simple model looks like this (but you can use a more advanced one if necessary):
p k= [px k, py k, pz k= p k-1+ dT. v <sub> k-1sub>
v k= v k-1
This can be put into matrix form like this:
[ 1 0 0 dT 0 0 ]
[ 0 1 0 0 dT 0 ]
[ 0 0 1 0 0 dT ]
x(k) = [ 0 0 0 1 0 0 ] . x(k-1)
[ 0 0 0 0 1 0 ]
[ 0 0 0 0 0 1 ]
Finally, you need to get the measurement function for your sensors. This measurement function is a way of measuring sensors when we know the state vector.
Your source A measures the speed of the system, so the measurement function looks like this:
h A( x k) = v k
Or in matrix form:
[ 0 0 0 1 0 0 ]
hA{x(k)} = [ 0 0 0 0 1 0 ] . x(k)
[ 0 0 0 0 0 1 ]
Your source B measures the position difference, so the measurement function looks like this:
h B( x k) = p k- p k-1= p k-1+ dT. v k-1- p k-1= dT. v k-1= dT. v <sub> ksub>
Or in matrix form:
[ 0 0 0 dT 0 0 ]
hB{ x(k) } = [ 0 0 0 0 dT 0 ] . x(k)
[ 0 0 0 0 0 dT ]
If you want to have a more accurate measurement function for source B, you may need to add the position in the previous time step to the state vector as shown below:
x k= [px k, py k, pz k, px k-1, py k-1, pz k-1, vx k, vy k, vz k]
Then the transition function will be:
[ 1 0 0 0 0 0 dT 0 0 ]
[ 0 1 0 0 0 0 0 dT 0 ]
[ 0 0 1 0 0 0 0 0 dT ]
x(k) = [ 0 0 0 1 0 0 0 0 0 ] . x(k-1)
[ 0 0 0 0 1 0 0 0 0 ]
[ 0 0 0 0 0 1 0 0 0 ]
[ 0 0 0 0 0 0 1 0 0 ]
[ 0 0 0 0 0 0 0 1 0 ]
[ 0 0 0 0 0 0 0 0 1 ]
The measurement function for source A becomes:
[ 0 0 0 0 0 0 1 0 0 ]
hA{x(k)} = [ 0 0 0 0 0 0 0 1 0 ] . x(k)
[ 0 0 0 0 0 0 0 0 1 ]
The measurement function for source B will look like this:
[ 1 0 0 -1 0 0 0 0 0 ]
hB{x(k)} = [ 0 1 0 0 -1 0 0 0 0 ] . x(k)
[ 0 0 1 0 0 -1 0 0 0 ]
In this case, the measurement of the speed of the system and the difference in its positions are almost the same. Since you never measure position, your condition may not be observed, which is likely to be a problem. For more information see the following documents:
source to share