Comments (10)
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.
from filterpy.
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.
from filterpy.
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!
from filterpy.
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.
from filterpy.
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!
from filterpy.
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?
from filterpy.
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)
from filterpy.
from filterpy.
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,
from filterpy.
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?
from filterpy.
Related Issues (20)
- FixedLagSmoother use of xSmooth is inconsistent and breaks Saver
- examples with different sensor types fused together? HOT 2
- Kalman filter example: wrong transition matrix?
- new release since version 1.4.5 ? HOT 3
- Error in RTS equations
- Bug in Cubature Kalman Filter : residual_x not used
- Release latest changes?
- Batch filter not working as expected
- error in batch_filter when using data with missing values HOT 2
- Kinematic Filter has same variance in all dimensions HOT 1
- EKF: Jacobian Matrix for the state estimate error covariance matrix (P) prediction
- Passing Q matrices to UKF's RTS smoother HOT 1
- FilterPy Version for Python 2.7
- residual_resample issue
- Does only the noise ratio matter in Kalman filters?
- Comment in documentation is out of date
- [question] Can the KalmanFilter handle missing data points? HOT 1
- LinAlgError: 1-th leading minor of the array is not positive definite HOT 1
- Optimize KF parameters
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from filterpy.