I'm working on a project to track the position of a camera which is mounted on a moving device through data-fusion. The data I get is
1) the velocity in x-, y- and z-direction of the camera from source A
2) the difference between the positions in the current and the last frame (in 2D, Z is not supposed to change either way) from source B
I already did a similiar project but without any data fusion or so and used the Kalman-Filter which is implemented in OpenCV.
Currently I keep getting confused by all the different implementation techniques I found online so far.
How would I go about inserting/combining the data I got into the different components of the KF/EKF?
I have this example for altering the OpenCV-KF to work as EKF. It looks a lot like what I need except for my object actually being the camera itself and also being able to move on y- and x-axis, taking turns, .... Also instead of pixel-coordinates of the object I got the above mentioned data. https://sites.google.com/site/timecontroll/home/extended-kalman-filtering-with-opencv
Unfortunately, I'm not exactly sure how to define my transition and measurement function and the corresponding matrix.
Let x(k) be the current state, deltaT the passed time since the last update.
For 1 it probably would be something like:
x(k) = x(k-1) + deltaT multiplied with velocity(k-1)
For 2:
x(k) = x(k-1) + differenceBetweenFrames
So let's assume both data-sources are weighted the same which makes me think that combined it would be something like:
x(k) = x(k-1) + (deltaT multiplied with velocity(k-1) + differenceBetweenFrames)/2
For measurements: I get the velocity in X, Y and Z direction through source A and the difference between the current and the last frame in X and Y coordinates through source B.
Does that make sense or am I already wrong about anything there? How would I have to adjust my transition and measurement matrix as well as update my current state accordingly?
You are on the right track, but you seem confused with the notions of transition function and measurement function.
The first thing to do is to clearly define what state vector you want to consider. In your case, the position [px, py, pz] and velocity [vx, vy, vz] are enough:
xk = [pxk, pyk, pzk, vxk, vyk, vzk]
The second thing is to choose a transition function to model the dynamics of your system. Note that this has nothing to do with sensor measurements : it is only the model of how the state of your system evolve with time. In the case of position and velocity, a very simple model is as follows (but you may choose to use a more advanced one if needed):
pk = [pxk, pyk, pzk] = pk-1 + dT . vk-1
vk = vk-1
This may be put in matrix form as follows:
[ 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 derive the measurement function for your sensors. This measurement function represents the way the sensor measurements can be inferred when we know the state vector.
Your source A measures the velocity of the system, hence the measurement function is as follows:
hA( xk ) = vk
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 differences, hence the measurement function is as follows:
hB( xk ) = pk - pk-1 = pk-1 + dT . vk-1 - pk-1 = dT . vk-1 = dT . vk
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 might want to add the position at previous time step in your state vector, as follows:
xk = [pxk, pyk, pzk, pxk-1, pyk-1, pzk-1,vxk, vyk, vzk]
The transition function would then become:
[ 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 would become :
[ 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 would become :
[ 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 ]
That being said, measuring the velocity of the system and the difference of its positions is almost the same. As you never measure position, your state might not be observable, which will probably be a problem. Have a look at the following documents for more details: