In our transition to the 8-state EKF, we added "hidden" states for accelerometer and gyroscope biases. Mathematically, these states exist in our vector $\mathbf{x}$, but a critical question remains: Does the information from our sensors actually reach these hidden states? In control theory, this is the problem of Observability. A system is observable if the current state can be determined from the output sensors in a finite amount of time. If a state is unobservable, the EKF can "guess," but the variance in the covariance matrix $P$ will grow forever because no measurement is providing a "correction" for that specific state.
In a linear discrete-time system, observability is determined by the Observability Matrix $\mathcal{O}$. For a system with $n$ states, the system is fully observable if $\mathcal{O}$ has full column rank ($rank = n$):
$$\mathcal{O} = \begin{bmatrix} \mathbf{J}_h \\ \mathbf{J}_h \mathbf{J}_f \\ \mathbf{J}_h \mathbf{J}_f^2 \\ \vdots \\ \mathbf{J}_h \mathbf{J}_f^{n-1} \end{bmatrix}$$
What the blocks mean. We never sense the state directly — at each step we get a measurement $\mathbf{y}_k = h(\mathbf{x}_k)$, and the dynamics $\mathbf{x}_{k+1}=f(\mathbf{x}_k)$ tie the steps together. Observability asks whether we can reconstruct one state (say today's $\mathbf{x}_0$) by watching a few successive measurements. Linearising about the current estimate ($\mathbf{H}=\mathbf{J}_h$, $\mathbf{F}=\mathbf{J}_f$), a measurement taken $k$ steps in the future depends on today's state through repeated propagation, $\mathbf{x}_k = \mathbf{F}^k\mathbf{x}_0$ (read $\mathbf{x}$ and $\mathbf{y}$ here as small perturbations $\delta\mathbf{x}, \delta\mathbf{y}$ about the estimate, since we have linearised):
$$\mathbf{y}_0 = \mathbf{H}\,\mathbf{x}_0,\qquad \mathbf{y}_1 = \mathbf{H}\mathbf{F}\,\mathbf{x}_0,\qquad \mathbf{y}_2 = \mathbf{H}\mathbf{F}^2\,\mathbf{x}_0,\ \dots$$
Stacking these is precisely $\mathcal{O}$: every block $\mathbf{H}\mathbf{F}^k = \partial \mathbf{y}_k/\partial\mathbf{x}_0$ is a sensitivity of the same unknown $\mathbf{x}_0$, just read out by a measurement taken later and later — "how much would a future reading move if I nudged today's state?". To recover all $n$ components of $\mathbf{x}_0$ we need a linearly independent column for every state, i.e. full column rank $n$ (e.g. $n=8$ for our 8-state system).
Why this exposes hidden states. $\mathbf{H}$ has zero columns for the biases — no sensor reads a bias directly. But $\mathbf{F}$ mixes states over time ($b_\omega\!\to\!\theta$, and $b_a\!\to\!v\!\to\!p$), so after a few propagations $\mathbf{H}\mathbf{F}^k$ grows a non-zero column in a bias slot. A non-zero column means "nudging that bias would change a future measurement" — which is exactly the definition of observable. A hidden state becomes observable the moment the dynamics carry its effect into something a sensor can see. (Because the EKF is non-linear, all of this is local — evaluated at the current estimate — and, as the caveat below stresses, along a trajectory.)
Frozen vs. moving — an important caveat. Writing $\mathbf{J}_f^{k}$ above uses a single, frozen linearisation. That is enough to expose the bias structure, but it hides one thing: with a single range beacon, full observability of position only appears once the bearing to the beacon changes — i.e. once the robot actually manoeuvres. The honest test therefore evaluates $\mathbf{J}_h,\mathbf{J}_f$ at each step along the path, giving the time-varying observability matrix $$\mathcal{O}=\begin{bmatrix}\mathbf{H}_0\\ \mathbf{H}_1\mathbf{F}_0\\ \mathbf{H}_2\mathbf{F}_1\mathbf{F}_0\\ \vdots\end{bmatrix},\qquad \mathbf{H}_k=\mathbf{J}_h(\mathbf{x}_k),\ \ \mathbf{F}_k=\mathbf{J}_f(\mathbf{x}_k).$$ The code cell below builds exactly this matrix; the ranks it prints are what the summary table reports.
$$\mathbf{J}_{h} = \begin{bmatrix} 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ \frac{p_1}{d} & \frac{p_2}{d} & 0 & 0 & 0 & 0 & 0 & 0 \end{bmatrix}$$
and
$$\mathbf{J}_f = \begin{bmatrix} 1 & 0 & \Delta t & 0 & \frac{1}{2}\Delta t^2 (-\tilde{a}_1 s - \tilde{a}_2 c) & -\frac{1}{2}\Delta t^2 c & \frac{1}{2}\Delta t^2 s & 0 \\ 0 & 1 & 0 & \Delta t & \frac{1}{2}\Delta t^2 (\tilde{a}_1 c - \tilde{a}_2 s) & -\frac{1}{2}\Delta t^2 s & -\frac{1}{2}\Delta t^2 c & 0 \\ 0 & 0 & 1 & 0 & \Delta t (-\tilde{a}_1 s - \tilde{a}_2 c) & -\Delta t c & \Delta t s & 0 \\ 0 & 0 & 0 & 1 & \Delta t (\tilde{a}_1 c - \tilde{a}_2 s) & -\Delta t s & -\Delta t c & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & -\Delta t \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix}$$
Let's isolate the Magnetometer's contribution. We look at the first row of $\mathbf{J}_h$ and its interaction with the 8-state $\mathbf{J}_f$.
Row 1 ($t=0$): The magnetometer directly observes $\theta$.$$\mathbf{r}_1 = \begin{bmatrix} 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \end{bmatrix}$$
Row 2 ($t=1$): Multiplying $\mathbf{r}_1 \cdot \mathbf{J}_f$ effectively "grabs" the 5th row of the state transition matrix:$$\mathbf{r}_2 = \begin{bmatrix} 0 & 0 & 0 & 0 & 1 & 0 & 0 & -\Delta t \end{bmatrix}$$
Since we have two linearly independent rows, we can solve for both $\theta$ and $b_\omega$. The EKF "sees" that if the heading doesn't match the integrated gyro rate, the difference must be the gyro bias.
Now we look at the Distance sensor. To keep the math clean, let $c_1 = \frac{p_1}{d}$ and $c_2 = \frac{p_2}{d}$.
The precise values matter less than the fact that these accelerometer-bias columns are now non-zero ($\propto \Delta t^2$) — that is exactly what makes $b_{a1}, b_{a2}$ observable.
The Dilemma: Notice that $p_1$ and $p_2$ are always tied together through the same $(c_1,c_2)$ direction. If the robot is stationary, $c_1$ and $c_2$ never change, so we have two position unknowns but only one "type" of distance equation. Without motion, the position component along the circle (tangential to the beacon) is unobservable from a single beacon.
Conclusion: a single beacon constrains only the radial coordinate (the distance itself); the tangential coordinate — and therefore the absolute $(p_1,p_2)$ position — stays unobservable until the bearing $(c_1,c_2)$ changes. That can only happen if the robot moves, which supplies a second, independent distance equation. This is the missing ingredient the next section makes precise: the magnetometer fixes orientation, but it takes motion to fix position.
Why can't a single distance sensor pin down position? Rotational symmetry. With range only, you could rotate the entire world — the robot's position, its velocity, and its heading — about the beacon, and every distance reading would stay exactly the same. The EKF cannot distinguish these rotated copies of reality.
Two different things are needed to remove this ambiguity:
The magnetometer fixes orientation. By providing an absolute reference for $\theta$, it pins the robot's heading to the world (and thereby makes the gyro bias observable). But on its own — while the robot is stationary — it still cannot say where on the circle of radius $d$ the robot sits: the tangential position remains unobservable. Even adding a ZUPT, the stationary system reaches only rank 7, not 8.
Motion fixes position. As the robot manoeuvres, the bearing to the beacon changes; watching the distance evolve under a known (magnetometer-anchored) heading and integrated velocity finally fixes the tangential coordinate. Only with genuine manoeuvres does the rank reach the full 8.
So the correct statement is: magnetometer + motion $\Rightarrow$ rank 8. A ZUPT while stationary calibrates the biases (forcing $v=0$ separates a constant accelerometer bias from a steady velocity) but does not resolve position-around-the-beacon — that still requires movement.
Big picture: For a professional navigation filter, sensors are not just sources of data; they are mathematical constraints. The magnetometer tells the distance sensor which way the robot faces, and motion tells it which way the robot has moved — together they let the EKF peer into the "hidden" world of sensor biases and pin down absolute position.
import numpy as np
# Time-varying local observability of the 8-state EKF
# state x = [p1, p2, v1, v2, theta, ba1, ba2, bw]
dt = 0.1
names = ['p1','p2','v1','v2','theta','ba1','ba2','bw']
def wrap(a): return (a + np.pi) % (2*np.pi) - np.pi
def f_step(x, u):
p1,p2,v1,v2,th,ba1,ba2,bw = x
a1,a2,w = u
a1c,a2c,wc = a1-ba1, a2-ba2, w-bw
c,s = np.cos(th), np.sin(th)
a1w, a2w = a1c*c - a2c*s, a1c*s + a2c*c
return np.array([p1+v1*dt+0.5*a1w*dt**2, p2+v2*dt+0.5*a2w*dt**2,
v1+a1w*dt, v2+a2w*dt, wrap(th+wc*dt), ba1, ba2, bw])
def Jf(x, u):
th,ba1,ba2 = x[4], x[5], x[6]
a1c,a2c = u[0]-ba1, u[1]-ba2
c,s = np.cos(th), np.sin(th)
F = np.eye(8); F[0,2]=dt; F[1,3]=dt
F[0,4]=0.5*dt**2*(-a1c*s-a2c*c); F[1,4]=0.5*dt**2*( a1c*c-a2c*s)
F[2,4]=dt*(-a1c*s-a2c*c); F[3,4]=dt*( a1c*c-a2c*s)
F[0,5]=-0.5*dt**2*c; F[0,6]= 0.5*dt**2*s
F[1,5]=-0.5*dt**2*s; F[1,6]=-0.5*dt**2*c
F[2,5]=-dt*c; F[2,6]= dt*s; F[3,5]=-dt*s; F[3,6]=-dt*c; F[4,7]=-dt
return F
def Jh(x, sensors):
p1,p2 = x[0], x[1]; d = np.hypot(p1,p2)
rows = {'dist':[p1/d,p2/d,0,0,0,0,0,0], 'mag':[0,0,0,0,1,0,0,0],
'v1':[0,0,1,0,0,0,0,0], 'v2':[0,0,0,1,0,0,0,0]}
return np.array([rows[r] for r in sensors], float)
def observability(x0, u_seq, sensors, N=40, tol=1e-7):
"""Stacked time-varying observability matrix O = [H_k * (F_{k-1}...F_0)]."""
x, Phi, blocks = x0.copy(), np.eye(8), []
for k in range(N):
blocks.append(Jh(x, sensors) @ Phi)
u = u_seq(k)
Phi = Jf(x, u) @ Phi
x = f_step(x, u)
O = np.vstack(blocks)
rank = np.linalg.matrix_rank(O, tol=tol)
_, _, Vt = np.linalg.svd(O)
null = Vt[rank:] # unobservable directions
return rank, null
x0 = np.array([3., 4., 0., 0., 0.6, 0., 0., 0.])
still = lambda k: (0., 0., 0.)
# genuine, time-varying manoeuvre (changing bearing to the beacon):
move = lambda k: (0.6*np.cos(0.3*k), 0.4*np.sin(0.2*k), 0.5*np.cos(0.15*k))
configs = [
("Distance Only (moving)", move, ['dist']),
("Magnetometer Only", move, ['mag']),
("Dist+Mag (stationary)", still, ['dist','mag']),
("Dist+Mag (moving)", move, ['dist','mag']),
("Dist+Mag+ZUPT (stationary)", still, ['dist','mag','v1','v2']),
]
print(f"{'Configuration':30s} rank unobservable mode(s)")
print("-"*78)
for name, u_seq, sensors in configs:
rank, null = observability(x0, u_seq, sensors)
if len(null) == 0:
modes = "(fully observable)"
else:
modes = "; ".join(
"{" + ", ".join(f"{n}:{round(val,2)}" for n, val in zip(names, v/np.max(np.abs(v)))
if abs(val) > 0.05) + "}"
for v in null)
print(f"{name:30s} {rank:>2d} {modes}")
The ranks below are those printed by the code cell above for a representative trajectory (an origin-referenced beacon, robot at $(3,4)$). "Moving" means a genuine manoeuvre — the bearing to the beacon must change; constant straight-line motion is not enough.
| Configuration | Rank ($\mathcal{O}$) | Observable States | Unobservable "Modes" | Filter Behavior |
|---|---|---|---|---|
| Distance Only (moving) | 7 | Range geometry, velocity, biases (up to a global rotation) | The absolute-orientation gauge: heading $\theta$ coupled with the tangential position | Everything is known only up to a rotation about the beacon; absolute heading drifts. |
| Magnetometer Only | 2 | Heading ($\theta$), Gyro Bias ($b_\omega$) | All positions $p$, velocities $v$, accel biases $b_a$ | Orientation is known, but position "hallucinates" drift as accel biases integrate. |
| Dist + Mag (stationary) | 5 | Radial position, $\theta$, $b_\omega$ | Tangential position + (velocity $\leftrightarrow$ accel-bias, entangled) | A steady velocity is indistinguishable from a constant accel bias. |
| Dist + Mag (moving) | 8 | All 8 States | None | Full convergence — manoeuvres change the bearing and break the rotational symmetry. |
| Dist + Mag + ZUPT (stationary) | 7 | $v(=0)$, $\theta$, $b_\omega$, accel biases | Tangential position (where you are on the circle) | Biases get calibrated, but position-around-the-beacon still needs motion. |
Note: ZUPT is not a substitute for motion. While stationary, even Dist + Mag + ZUPT reaches only rank 7: it forces $v=0$ so the accelerometer biases separate out, but it cannot tell where on the circle around the beacon the robot sits. Resolving that last tangential degree of freedom requires actually moving (changing the bearing).
Caveat on exact numbers: these ranks are local and trajectory-dependent. Poorly exciting motions (e.g. constant straight-line driving) can leave additional modes weakly observable; re-run the code cell with different
u_seqinputs to see this.