# Dealing with an extra state in a Kalman filter

I'm working up a Kalman filter for a system that has a body* moving on a sphere a constant distance from 0,0,0, on the surface of the earth (and, hence, with a distinct gravity vector).

I have a 6-axis IMU on the body.

I wish to track the motion of the body as best as I may.

The problem is that without an external angular reference, which I do not have (no compass), I have no "north" reference (by "north" I mean a knowledge of the absolute direction level to the ground). So the system is not fully observable. However, I still need to describe the body motion in 3D.

I'm willing to allow the system to wander in it's "north" estimate, but I'm wrestling with how "not able to observe the compass direction" translates to expressing the situation in the Cartesian coordinates in which I'm describing the motion.

If I just formulate my filter naively, then I'm left with having an uncertainty for the direction "north" that climbs as the filter evolves (I can artificially start it with zero, or at least small, uncertainty).

Because I'm reconstructing events that don't last forever, I may be able to just let the uncertainty climb as time goes on. But I can see at least numerical difficulties as the one eigenvalue in my covariance matrix climbs without bound.

Is there a way that this can be dealt with? The math just doesn't work if I arbitrarily remove north-facing positions and velocities from my model -- I need a way of expressing the model that lets the rotation around vertical be somehow taken out of the covariance matrix, while still keeping the other two rotations in place.

* It happens to be a control line model aircraft, but that's immaterial to my problem today.

I figured it out, at least in principle:

Given a state vector $$x = \left [\vec x\ \ \vec v \ \ \vec \theta\ \ \hat k g \right ]^T$$, where $$\vec x$$ is the position of the IMU with respect to the pivot point, $$\vec v = \frac{d}{dt} \vec x$$, $$\vec \theta$$ is the rotation of the IMU coordinate system from the problem coordinate system, and $$\hat k g$$ is the acceleration due to gravity (which I think I need to resolve offsets in the vertical direction).

Then the system model becomes $$\begin{split}x_k = f \left( x_{k-1}, u_k \right) \\ v_r = h \left( x_{k-1}, u_k \right) = \frac{\vec x \vec v}{\| \vec x \|} \end{split}$$

And the "measurement" is just the assertion that the distance from the center of the sphere to the IMU does not change: $${v_r}_m = 0$$

At any point, let $$F, G, H$$ represent the linearization of $$f$$ and $$h$$.

Just going on intuition, I believe that as long as the IMU is maneuvering sufficiently, the Kalman filter will be able to observe everything except the compass direction of the projection of $$\vec x$$ onto the ground -- my original question is how to work around this.

My answer is to come up with a $$9 \times 10$$ matrix $$T$$, computed from $$\vec x_k$$, such that $$x_k' = T x_k$$, which rotates $$\vec x$$, $$\vec v$$ and $$\vec \omega$$ by the same amount, such that the $$\hat i$$ - facing component of $$\vec x'$$ would be zero -- but then it removes the equivalent row from $$T$$ altogether. This gives a $$x'$$ which has nine elements instead of ten -- we've made the compass direction of $$x$$ into a don't-care, and thrown it away.

Then all of the Kalman calculations can be done on $$x'$$. There's some not-inconsiderable i's to dot and t's to cross because I can't just use $$F' = FT$$ at each step. However, this gives me a reduced-order system with dynamics that should be observable in a satisfactory way.

According to your description I deduce that your system not fully observable. So unless you add additions sensors, which makes your system fully observable, I believe there is nothing else you can do to improve the estimate of "north".

One cheat that could artificially maybe keep the covariance smaller it by ensuring that the contribution of the process covariance to the north-state is zero (or really small). This however won't improve the estimate of north. Another option is to reset the covariance at regular time intervals or when the largest eigenvalue of the covariance exceeds a certain threshold. There might be some clever ways to do this reset such that you lose no or minimal uncertainty information of the other states.

A very different option might to use a particle filter instead of a Kalman filter. Namely I assume that north should be a direction, so can always be represented with angle in the interval between zero and 360 degrees, therefore the uncertainty should also be bounded by this interval. So for a particle filter you can use the modulo operation to ensure this interval.

• I can live without the "north" direction being correct in the results -- it's how I get the thing to be accurate with all of the knowns that I'm struggling with. Aug 2, 2019 at 2:46