Kalman filter prototype

[ayberkozgur]

This PR is an early preview of Kalman filtering to 6D tag pose. Comes with hardcore c++11 std::map::emplace() and related syntax :)

Is a partial solution to #19.

For now, the quick & dirty covariance calibration seems to work decent enough. Please test further.

A huge issue now is gimbal lock and discontinuities in angle. We need quaternions to fix this. done

In the near future, more goodies such as

• Calibration API done
• Enable/disable on demand API done
• Quaternions done
• IMU integration done, see also https://github.com/chili-epfl/qml-imu
• Optimizations pretty much done
• Sample done
• Remove Filter completely done
• Add tag discard mechanism done
• Fully document all math involved done

will follow.

[ayberkozgur]

We need a comparison to the existing Filter. Discussion with @severin-lemaignan yielded these points:

• Filter is simple and easy to maintain
• Kalman filter has the possibility to "smooth" certain state components more than others (though this functionality could be added to Filter as well)
• Kalman filter is well grounded in statistics and should have probabilistic benefits, though we don't fully understand how; things I personally fully understand as of now:
  • Has enough expressive power to incorporate Gaussian noise models of the process, measurement and control, which would be accessible in case of e.g IMU
  • Is able to statistically predict when there is no observation (we could hack it into Filter but this would probably be very ugly); we could use this to implement a very cheap performance mode where e.g detection is done very occasionally, tracking is done every tens of frames and the rest is predicted by the filter

With these, we need to decide whether to keep Filter, Kalman filter or both (this would probably be the least favorable case). In the meantime, I will again read up on KF and fully understand the statistics behind it.

[qbonnard]

Ah I forgot to answer this conversation, but it seems pretty obvious that Filter is redundant in this context. RIP Filter, we liked you, you were simple and easy to maintain.

Other than that, should I start being annoying being exigent reviewing it, or are there more commits to come ?

[ayberkozgur]

So we decide to remove Filter, good. There will be more commits to follow, especially the processing of the data coming from https://github.com/chili-epfl/qml-imu.

One thing to note is that the float/double difference of tag poses go beyond 10^-5% as you might have seen from the failed Travis build. We need to consider maybe lowering this limit or accepting this difference because it is not likely to go down.

[qbonnard]

Hum that's funny. I haven't looked at the changes yet, so I'll just ask: is this test fail due to the PR or to the machine running the Travis thing ?

[ayberkozgur]

This is due to the matrix inversion and multiplications in the PR. That's why it's not likely to go down.

[qbonnard]

Well it's really cool to have a test that catches that then. If I remember Numerical Analysis correctly, an error at 10^-5 means +/- 0.1 pixels on a thousand... which is more or less the limit on the (2D) detection in "normal" cases. Si if we really want to be precise, we should use doubles, otherwise floats are OK. Again, it's cool to have a test that "documents" this limit !

[ayberkozgur]

One thing I noticed is that the matrix elements that have this critical difference are very close to zero. If you look at the translation components for example, they are perfectly accurate. Maybe we should not calculate the difference threshold as relative to compared elements, but as absolute depending on the expected range of compared elements; e.g 1e^-5 absolute for rotation elements since they are in [-1,1] and 1e^-2 millimeters absolute for translation elements which are in 100's of millimeters range most of the time, if not 1000's.

[qbonnard]

That makes sense. In the end what we care about is the number of significant figures, not an absolute precision...

On Sun, Dec 7, 2014 at 5:01 PM, Ayberk Özgür notifications@github.com wrote:

One thing I noticed is that the matrix elements that have this critical difference are very close to zero. If you look at the translation components for example, they are perfectly accurate. Maybe we should not calculate the difference threshold as relative to compared elements, but as an absolute depending on the range of compared elements; e.g 1e^-5 absolute for rotation elements since they are in [-1,1] and 1e^-2 millimeters absolute for translation elements which are in 100's of millimeters range most of the time, if not 1000's.

— Reply to this email directly or view it on GitHub https://github.com/chili-epfl/chilitags/pull/75#issuecomment-65941987.

[ayberkozgur]

Now that we're removing Filter, we need a way of discarding tags that are not detected for a while/detected with large error. I think KF could be exploited very nicely here.

It involves a prediction and correction step where the state (pose of the tag) covariance estimate naturally "increases" during the prediction step and "decreases" during the correction step if "noiseless" observations are done. We do the correction step only when the tag is observed. This means that we can basically take the covariance estimate and discard the tag history if it gets too large.

How large is up to the user to calibrate and I think that the magnitude can be taken as the trace of the covariance matrix, up to some coefficients for different diagonal elements since quaternion components and position components have different scales/units. We can even compare it to the trace of the process noise covariance matrix (Q).

What do you guys think?

[ayberkozgur]

I'm done except for documentation. Awaiting your reviews.

[ayberkozgur]

BTW, about the samples demonstrating the IMU API: There will be none in chilitags because this is too platform-specific since it requires access to sensors. There will be one sample in qimchi that uses https://github.com/chili-epfl/qml-imu which can be previewed here: https://github.com/chili-epfl/qimchi/tree/imu.

[qbonnard]

Awaiting your reviews.

Cool ! I should have some time tomorrow. Round one... ;)

[covariance stuff] What do you guys think?

That sounds like black magic to me... Maybe to the user too, unless we can explain how to set the threshold in less than two paragraphs involving Kalman's internals. How does it compare to predicting the position with Kalman for n frames ?

[ayberkozgur]

Actually, it's more than black magic. The covariance matrix estimate is an accurate and optimal measure (up to the accurateness of process (Q) and observation (R) noises tuned by someone) of how imprecise the current tag pose estimate is. Then, trace is a nice way of reducing this to a single number (there may be others such as det but I have to look for the others).

If you look at the KF equations, you'll notice that the covariance estimate only depends on Q and R which are constant for the moment. For the current case, this is partly why it corresponds roughly to the number of frames. Q and R are actually Q(t) and R(t) that we can determine in the future according to various factors. One off the top of my head is attributing more noise to tracking than detection or more noise when the device is shaking. In that case, we can replace the current "scale by geometric mean of Q and R diag entries" with something more appropriate and it will make more sense and be less relevant to the number of frames.

Edit: Also about explaining the internals of KF, there is https://github.com/ayberkozgur/chilitags/commit/f24e1f1ae541c8ff9b69987f059ef41cd929cec9. That doesn't explain the "scale by geometric mean of Q and R" thing that I cooked up in 20 minutes that seems to work well, but still I don't think that tuning one number (persistence) while observing the detection behavior is too hard if the user is really uninterested about the internals of the filter. If it's really that hard, then they can just use the default value.

[qbonnard]

Okay okay... let's not stall development too much, I already add way too much latency :s So here is the deal: I trust you on quaternions and you trust me on 2D ;) I rebased this PR, left quaternions untouched, added the minor documentation fixes and unremoved the filter for the 2D detection: https://github.com/qbonnard/chilitags/commits/kalman Does it work for you ? If so I can push to chili's master

[ayberkozgur]

Everything seems to be OK.

[johnnyx811]

guys I missed some messages, but quaternions have singularities...you should use exponential maps

2015-01-05 9:24 GMT+01:00 Ayberk Özgür notifications@github.com:

Everything seems to be OK.

— Reply to this email directly or view it on GitHub https://github.com/chili-epfl/chilitags/pull/75#issuecomment-68679347.

[ayberkozgur]

As far as I know, there are no singularities in quaternions, that's one of the main reasons why we prefer them (sometimes) over Euler or angle-axis representations.

[johnnyx811]

sry, you are right. Now I remember: there is still one argument for replacing quaternions with exponential maps in linear Kalman filtering. 3D rotations have 3 degrees of freedoms, quaternions have 4 dimensions, exponential maps have 3. If I remember correctly in quaternions you have a problem of scale which may suggest some sort of normalizations. This problem is solved in exponential maps.

I don't remember exactly all the pros and cons for this rotation representation: but here they should discuss them. http://www.cs.cmu.edu/~spiff/moedit99/expmap.pdf

All the best

Andrea

2015-01-05 9:37 GMT+01:00 Ayberk Özgür notifications@github.com:

As far as I know, there are no singularities in quaternions, that's one of the main reasons why we prefer them over Euler or angle-axis representations.

— Reply to this email directly or view it on GitHub https://github.com/chili-epfl/chilitags/pull/75#issuecomment-68680333.

[ayberkozgur]

As far as I know, the exponential map concept is exactly what we know as the 3x3 rotation matrix. Due to redundancies, it still has 3 DOF despite storing 9 values. Quaternions store 4 values but they are unit quaternions so one DOF is reduced and they still have 3 DOF. The pro of using 3x3 rotation matrices is that we can do matrix multiplications and combine rotations/rotate vectors very cheaply. This doesn't translate well into things like Kalman filters where we don't have to combine rotations or rotate vectors but we have to process rotations as a state vector. That's why we (at least I) don't use 9 values in the state vector because it's overkill.

[ayberkozgur]

Having read the exponential part of the paper you provided, I would say the concept described there is interesting. It is only very slightly different than what we use (unit quaternions). Still, we (at least I) do a sinc-like epsilon check whenever we would use the norm of the vector part of the quaternion that is the core idea of the slight modification described in the paper; you can see it in https://github.com/ayberkozgur/chilitags/blob/kalman/src/Filter3D.cpp#L280 and https://github.com/ayberkozgur/chilitags/blob/kalman/src/Filter3D.cpp#L297.

[johnnyx811]

yep that's correct and you divide by quaternion norm. now you have three independent components (required from the classic Kalman Filter) instead of four.

2015-01-05 11:12 GMT+01:00 Ayberk Özgür notifications@github.com:

Having read the exponential part of the paper you provided, I would say the concept described there is interesting. It is only very slightly different than what we use (unit quaternions). Still, we (at least I) do a sinc-like epsilon check that is the core idea of the slight modification described in the paper; you can see it in https://github.com/ayberkozgur/chilitags/blob/kalman/src/Filter3D.cpp#L280 and https://github.com/ayberkozgur/chilitags/blob/kalman/src/Filter3D.cpp#L297 .

— Reply to this email directly or view it on GitHub https://github.com/chili-epfl/chilitags/pull/75#issuecomment-68689009.

[qbonnard]

@johnnyx811 I think you mixed up your dates. 1st of January is time for new Years' resolution, not April Fools' jokes... Or did you have a bad break up with Rodrigues ? ;)

[qbonnard]

@ayberkozgur I was thinking that it would be nice to have some test or evaluation of Kalman before merging this, but this may take a big while... How annoying is it to wait more ?

[ayberkozgur]

Well, most probably I won't be developing Chilitags until somewhere around the end of March because I have to publish; I'll probably get fired otherwise :)

[severin-lemaignan]

@ayberkozgur I confirm :-)

[qbonnard]

Well if you get fired coding Chilitags, you'll have more time for more coding. That's a double win for Chilitags ;) Just use a fake GitHub account, I'm sure no one will notice... Join me to the dark side of no publications. We have cookies.

So it's not a problem if I stall this until end of March then ?

[ayberkozgur]

It's easy for you to say, you already have your PhD :)

I don't see the point of delaying this for three months for added tests. I can understand your desire to have tests for all new features but the current PR is quite big as it is. It would better be suited as an issue for now (not to mention there is another PR waiting on this one).

[qbonnard]

Finally merged this PR, since I probably won't add to it before March. R.I.P. hopes of tests ;)

Many thanks again !