Will an Unscented Kalman Filter Be "As Good" as Other Optimization Algorithms for This Problem?

Question

I want to calibrate a tri-axis magnetometer when a tri-axis gyroscope is also available.

I am fairly certain I can solve this problem using various optimisation algorithms, but I would prefer to use an unscented Kalman filter for a number of reasons that I'll discuss later, but in short: because it is online, allows change, and explicitly exposes uncertainty.

However, a Kalman filter seems mysterious here! – I don't really see how its covariance matrix can store enough information about what has been determined and what remains uncertain such that its recursive process can eliminate the now unlikely possibilities.

I think my questions boil down to:

1. Will this problem be solved well with an unscented Kalman filter?
2. Will a Kalman filter be "as good" as a batch / offline optimisation algorithm?
3. How should I add a normalisation step to the Kalman filter (see later)?

A further point I would love to understand: I assume that there are optimisations problems where a (non linear) Kalman filter is not suitable and other techniques must be used. What factors should I use to decide this?

Background

A tri-axis magnetometer produces time varying 3 vector samples $m_k$. These are in the "body frame". They contain noise. Due to sensor calibration, device construction, and soft and hard iron effects around the sensor: The measurements are highly biased away from the origin; The measurements may not properly align to the body coordinate system; The vector components may not all have the same gain and their axis vectors may not be orthogonal to each other. All this means that the uncalibrated sensor will not give a useful indication of the true local field $h$.

A method for calibrating a magnetometer is to rotate the device having the magnetometer to numerous poses in a constant magnetic field and build up a large set of samples. The samples will all be found to lie on an ellipsoid. A best fit is performed, and a mapping to turn the ellipsoid in to a unit sphere centred on the origin is determined.

I suspect some disadvantages exist with this approach:

• The magnetic field may not be constant during this process.
• A good ellipsoid fit depends on fairly uniform sampling of the ellipsoid surface, perhaps particularly at its "extreme" points. This may not happen during a human assisted calibration.
• The process does not account for a rotation between the magnetometer and the device's body frame.

Calibrating with Gyroscope

It seems to me that it ought to be possible to get a more reliable, more accurate, and more rapid calibration if a gyroscope is also available.

I believe that all this can be modelled with:

$h = R_k M (m_k-b)$

Where:

1. $h$ is the true local field (and not the Kalman observation function that I'll use $\mathbf{h}$ for – sorry the overloaded terminology);
2. $R_k$ a 3x3 time varying rotation matrix transforming from body coordinates to world coordinates;
3. $M$ a 3x3 matrix that corrects the tri-axis magnetometer's gains, rotation, non-orthogonality, and hard / soft iron.
4. $m_k$ is an instantaneous sample from the magnetometer;
5. $b$ is the magnetometer bias.

There is no limit on scaling here, so I would add the constraint that $M$ is normalised with $\det(M)=1$

I assume that $R_k$ can be obtained from the gyroscopes by integration. On the device I'm using it seems to have low enough drift.

I could collect "enough" data like this, and then perform an optimisation to obtain a best fit. All the samples should result in the same $h$, so this should give a way to determine the unknown parameters.

However, I would like to use an unscented Kalman filter.

• The Kalman filter should allow for $h$ to change during calibration (due to local field perturbation) and similarly allow for a degree of drift in $R_k$.
• It would mean that I would have a measure of the certainty of all the estimated parameters, and that I could run the process until the parameters are "certain enough", or simply leave it running all the time.
• It'll also mean that during calibration I'll have useful preliminary results available that can be shown to the user.
• It may be possible to provide instruction feedback to the user derived from the covariance matrix. This would direct the user to move the device in a way that will much more quickly reduce the known remaining uncertainty.

Kalman Filter Model

Taking the form of the process and measurement models from Unscented Kalman Filter Tutorial – Terejanu

$x_k = \mathbf{f}(x_{k-1}) + w_{k-1}$

$z_k = \mathbf{h}(x_k) + v_k$

The Kalman filter process model here is the identity: $\mathbf{f} = \mathbf{I}$. There isn't any process beyond process noise, so $x_k = x_{k-1} + w_{k-1}$. I believe the process noise is only on $h_k$ – the other parameters should be constant over the expected time scale.

Everything happens in the measurement model. $z_k$ is just the magnetometer reading $m_k$. So, $z_k = m_k = M_k^{-1} R_k^{-1} h_k + b_k + v_k$

It may make sense to maintain $M_k^{-1}$ in the state variable $x_k$, rather than $M_k$. As $R_k$ is a rotation, $R_k^{-1} = R_k^\top$

Initial State

• $M_0$ can be initialised as $I$.
• $b_0$ can be initialised as the first sensor reading $m_0$, as the bias seems to have greater magnitude than the signal.
• $h_0$ can be the zero vector.

Initial Variance

• The diagonal terms of $M$ will have a small variance to allow for differential sensor gain because $\cos(\theta) \approx 1$ for the small $\theta$ angles that are expected.
• The off diagonal terms would have variance depending on the maximum expected $\theta$ as $\sin(\theta) \approx \theta$.
• Variance for $h$ should depend on the magnitude of the expected field.
• Variance of $b$ – I'm not sure!
• I presume the initial covariances can be 0?

Summary

The question is – will this work as a Kalman filter and give useful results? Will it work at least as well as batch optimisation? Or have I got completely the wrong end of the stick?

Also – where should I add normalisation such that $\det(M) = 1$?

Implementing this with Lapack to get it running on an iPhone seems like it will be quite an investment, so I'd like to feel fairly sure that it will work before I begin!

Progress

• I've built a dirt simple App to capture data from my test device (an iPhone).
• I've imported the data in to Julia.
• I'm going to see if I can use some Julia packages implementing general optimisation and Kalman filtering to determine the model parameters from the data I've captured, and hopefully see which works better.
• (25/1/16) I've got the optimisation version possibly working. The "corrected" readings appear to have more (gyro) drift than I'd expect over the capture timescale, so I think I'm going to need to revisit / check my assumptions on that.

Answer 1

Though this is quite an old question, my two cents might help others in the future.

The UKF is a very nice algorithm, but you need to take care before using it. My concerns are the following:

You want to estimate matrices with constraints, but the UKF is for Euclidean manifolds. The problem here is that when you impose a constraint, you are not longer on a Euclidean manifold. This means that the theory will not support you.

If you want to enforce the constraint then I suggest that you choose a parametrization for $M$ that intrinsically enforces the constraint and you do not need to care about that.

Of course, you can normalize $M$ in the brute force way (by calculating $\det M$ and dividing by its cube root.

A good example for this (and if you want to have a precise $R$ matrix, you might want to estimate it with another UKF) is the quaternionic Unscented Kalman Filter, which is for Riemannian manifolds (i.e., for quaternions - a parametrization of rotations with a unit norm constraint). I suggest you look into papers like Unscented Kalman Filters for Riemannian State-Space Systems.

Another suggestion: do not initialize the covariance to 0. Bayes' rule cannot help you if your prior has zero probability, it will remain 0.