How to add a magnetometer in an Extended Kalman filter for innovation update?

Question

I can't find out the response so I am posting here.

My post kind of follow this one : Adding magnetic field vector to a Kalman filter but I already know that I don't have to put the magnetometer in a state vector.

I am working on a implementation of an Extended Kalman filter. So far I have integrated a gyroscope, an accelerometer and encoders wheels.

But I don't know what are the equation for the update state to integrate a magnetometer.

So far :

• I have seen some kind of work where magnetometer are fuse with accelerometer. But I don't have any formulas and I am kind of stuck...

• I have seen some kind of work where there is no quaternion but I can't afford this modelisation cause of gimbal lock and I have all my work already in quaternion...

Thanks in advance for help. Don't hesitate if you need complementary details.

Answer 1

A magnetometer measures the local magnetic field. When assuming that the Earth is the only magnetic source, then this magnetic field should be constant in world coordinates. But the magnetometer measures the magnetic field in local body fixed coordinates. The world and body coordinates should be related via the unit quaternion. So the magnetometer measures a constant vector rotated by the quaternion. In order to find this constant vector you would need to do some calibration at the start of each flight.

Answer 2

First, you need a model of the magnetic field you are measuring. Have a look at the IGRF model, which is usually used in space applications. There are ready to go implementations for most programming languages.

With this model, you are able to calculate a theoretical magnetic field vector in the inertial coordinate frame, depending on your estimated position. With your estimated quaternion you transform this vector in your body coordinate frame.

Second, now you have a theoretical vector in the body frame and a measured vector in the body frame, provided by the sensor.

The rest is straight forward EKF formulation.

References: Three-axis attitude determination via Kalman filtering of magnetometer data, MARK L. PSIAKI

Fundamentals of Spacecraft Attitude Determination and Control, Markley, F. Landis

Answer 3

If you want to incorporate magnetometer measurements in an EKF, you must model the magnetometer. In other words, you need an equation $\mathbf{y} = \mathbf{h}(\mathbf{x})$ where $\mathbf{y}$ is your magnetometer measurements or some transformation of the magnetometer measurements (e.g., you can preprocess your actual measurements) and $\mathbf{x}$ is your state vector. This is called the observation model and is included in the standard EKF equations.

Now, you might already understand that part, and if so, I assume you don't understand how to come up with the observation model. I can provide you with some direction on what to do:

1. What states can you measure using a magnetometer with some preprocessing? Can you measure any position states (e.g., x, y, or z)? Can you measure orientation states (roll, pitch, or yaw)? I will give you a hint. On Earth, you can estimate yaw using the magnetometer.
2. If you can process the measurements to find yaw, you can write your observation model, then plug the observation model in your EKF.
3. If you want to use in real life, you need to calibrate your magnetometer and do some outlier removal (e.g., do sanity check on yaw estimates from magnetometer). This is an absolute must for real life applications if you want the system to work.

If you need help processing the magnetometer measurements, I recommend this article for a simple model. Many other references exist, but this one is straightforward for getting started.

If you have specific question/confusion about the article or details in processing the magnetometer measurements. I can add some detail to my answer.