Example: Quaternion in UKF

[DylanWhite2]

Does anyone have an example of using a gyroscope (or other rotation sensor) to drive the estimation of a quaternion in the state? I understand that a quaternion has issues with the covariance due to the overparameterization of 3 dimensions in 4 values. Also that we cannot simply add quaternions for averaging.

Any example of a quaternion with filterpy (UKF) would be great. Thanks.

[liam-b]

Yeah, I'm also stuck on this. It seems surprisingly tricky considering I would imagine it's a very common use case. An example would be really helpful.

[DylanWhite2]

@liam-b

The approach I've taken, but I'm not sure is strictly correct but is "working" for me for now, is to use Modified Rodrigues Parameters in the filter state (which is a minimal vector representation). MRP have singularities at 2pi (but there is something called the shadow MRP which can be used to avoid this from what I understand)

In the predict step, I convert the MRP to a unit quaternion and then propagate the quaternion using the gyroscope angular velocities. I then convert the quaternion back to MRP to update the state. (Then whenever I want to have a quick reference for myself I convert the MRP to Euler/Tait-Bryan angles)

Note, that for my use case I am never observing the orientation in the update step. So I'm not sure how that would affect things.

I spent a lot of time looking at error-state Kalman filters and multiplicative UKFs with quaternion dynamics, but just couldn't get my head around it. This was the easiest thing I came up with that lets me continue for now.

[rlabbe]

I understand the frustration, but this is a hard problem with no single 'plug in' answer. filterpy is a pedagogical library, intended to get to you to the point of being able to read the literature and implement the answers.

There has been a lot of research on this in the last 20 years. Why is it hard? You need at least 4 parameters to represent a rotation without singularities. this was proven in 1964 by Stuelpnagel. The most common choice for that is unit quaternions.

Okay, no problem, right? Slap together an example already Roger! Ya, well. It's not so easy. Unit quaternions are in R^4 (4D real number Euclidean space), but the rotations are on a 3D surface - you are basically projecting a 4D hypersphere into 3D space. It is not Euclidean anymore, and KFs are based on Euclidean type space. The errors are l2 norms - Pythagoras theorem, which only works in Euclidean space. But quaternions are manifolds - if you 'zoom in' enough, they 'look' like they are in R^3 (Euclidean), but as you zoom out they aren't. Think about trying to map the surface of the earth on a flat sheet of paper. You can define a mapping, such as the stereographic projection (point above north pole, draw lines to surface of earth, put a piece of paper in between and draw points where the lines meet the paper). You can map 1/2 the world that way, but if you draw a line to the southern hemisphere it also passes through the Northern hemisphere - 2 points map to 1. That fails "bijection" a requirement for manifolds that there is a 1 to 1 mapping. Another problem - if you take a paper map of your town and draw a line from the ice cream store to the tack factory the length of the line correctly equals the distance between the two. Draw a line between England and LA on a map of the world and the line length is not close to the distance between the two cities (you need to draw a great circle). It is not homeomorphic at that scale. Hand wave, hand wave, the same issue occurs when projecting 4D quaternions onto a 3D sphere. In practice this leads to things like singularities in the covariance matrix after a few updates, and at best your filter diverges, at worst it throws an exception. Opps. Another way to think about the problems - you rotate a quaternion by multiplying by another quaternion. The KF is adding values. You don't get a quaternion out the other side after an update. I haven't mentioned everything, but I'm not writing a dissertation here. It's really hard, such that NASA et. al. are regularly researching and publishing on the topic.

There are many different approaches to solve this. You can add a constrained minimization solve in the Update step, you can forcibly normalize the quaternion every step, and so on. They all have issues and difficulties. There is no one neat answer like we have with linear Euclidean problems with Gaussian errors problems. A lot of the literature is solving for spacecraft, which have very 'smooth' behavior with easily modelled non-linearities. Compare this to a small UAV subjected to buffeting winds and jerky human control inputs. You are going to need different math. A lot of the recent papers are addressing the UAV problem.

So far the best paper that describes all of this is https://www.ncbi.nlm.nih.gov/pmc/articles/PMC6339217/#B8-sensors-19-00149. It gives a proposed solution, but so do many papers, I have no idea if it is good or not, and for what problem domains. It is just written in a very clear style, and gives really good references.

You'll want to look at publications by Krassidis and Markley for the last 20 years or so to really get into this. And then the papers they cite.

The good news is that this is what I'm doing at work right now. But that is company IP, I can't and won't release it. Whether I'll make any open source contributions to this problem space is an open question. It is an overwhelming amount of work for a side project with no support. The other bad news is that I don't think there is one solution, one easy 'example' I can slap online and then you can go off and write a KF for your quadcopter or whatever. There are inherent mathematical difficulties that you need to understand. You need to understand manifolds, charts, homeomorphism, etc., and how to use them to understand how quaternions (or any angle representation) map into R^3 so you can understand where the KF will fail.

[DylanWhite2]

Thanks for the reply @rlabbe. And thanks for the hint towards that paper. Filterpy has been extremely useful in the early stages of my masters research!

[AlexAtCCRI]

I just stumbled upon this, question. it might be helpful to look at 'geometric algebra' which provides some really nice interpretations for quaternions (and lie groups). https://marctenbosch.com/quaternions/. also nice website bivector.net. disclaimer: i worked on python package clifford for a while, so i am biased!

[rlabbe]

Nice, @AlexAtCCRI . I just gave a tutorial on manifolds and Lie groups a few weeks ago at work as we are transitioning to these for the non-linear solver ceres.

But as it turns out we will not be using quaternions so the chances of me doing a quaternion KF anytime soon is low. Maybe I'll get bored one day...

A question - I'm looking at clifford now, and I've been looking for a package that produces nice 3D cartesian plots with planes, spheres, vectors, etc. I was wondering if you would share how you generated these figures in your tutorials?

[AlexAtCCRI]

group theory is awesome.

which figures are you referring? i think i took them mostly from wiki, or made them. there is a Geometric Algebra package ganja.js which has a lot of visualization abilities, and there is a small amount of integration with clifford.

here are some examples of ganja.
https://enkimute.github.io/ganja.js/examples/coffeeshop.html

(also this is my work account, i am @arsenovic)

[rlabbe]

Like, for example https://clifford.readthedocs.io/en/latest/_images/blades.png

or the figure under Reflections on this page: https://clifford.readthedocs.io/en/latest/tutorials/g3-algebra-of-space.html

Sometimes I just want to do simple images showing lines intersecting through planes or maybe a plane through a trapezoid, and so on, with some labels and such, without having to do all the math of figuring out where to start/stop the plane or whatever, just fit everything. I do like programming it, in that reproducibility is easier than hand drawing in some app if you want to make changes or similar charts.

On Wed, Feb 15, 2023 at 5:32 AM AlexAtCCRI @.***> wrote:

group theory is awesome.

which figures are you referring? i think i took them mostly from wiki, or made them. there is a Geometric Algebra package ganja.js which has a lot of visualization abilities, and there is a small amount of integration with clifford.

here are some examples of ganja. https://enkimute.github.io/ganja.js/examples/coffeeshop.html

— Reply to this email directly, view it on GitHub https://github.com/rlabbe/filterpy/issues/283#issuecomment-1431375543, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABIP5323MQPI4CHB3IGH3ZTWXTLE7ANCNFSM6AAAAAASQJPBJU . You are receiving this because you were mentioned.Message ID: @.***>

[AlexAtCCRI]

i dont think i made either of those images, unfortunately. for me programming diagrams takes too long, and results were not as good.

i generally use inkskape for diagrams, but i am old-school in that regard.
stephen (author of ganja) has some really nice modern tools https://enki.ws/ganja.js/examples/pga_dyn.html.

i think all the plots/images in filterpy docs are really nice. although i like this slightly different one of the kalman diagram,

[tugrul512bit]

Is it viable to do this:

• convert a quaternion to a rigid-body with N points in cartesian-space with specific orientation (as if you rotated a 3D object)
• filter each joint of body separately in N Kalman-filters or as a linear-system solver in single Kalman-filter
• apply constraints of rigid-body (i.e. iteratively translate joints until it is rigid again)
• convert rigid-body back to quaternion as output

would it cause too high error or even diverge?