Motion Models

factors::motion — what the platform’s own dynamics assert, with no sensor involved.

That makes them unusually cheap and unusually easy to misapply. Each is valid only while its condition holds, so it is added by whatever detects that condition: a stationarity detector for ZUPT and ZARU, vehicle knowledge for the other two. Applied where the assumption is violated they are confidently wrong, in the way only a near-zero-noise constraint can be.

ZeroVelocityFactor — ZUPT

The platform is at rest. Applying this bounds the drift inertial integration would otherwise accumulate through a stop, at no sensor cost.

Blocks $[{\text{SE}}_2(3)\text{state}]$ — residual 3, Jacobian $3\times 9$.

$$\mathbf{r}={\mathbf{v}}_{\text{world}},\frac{\partial \mathbf{r}}{\partial (\delta \mathbf{\rho },\delta \mathbf{\theta },\delta \mathbf{\nu })}=\left[0\right|0\left|R\right]$$

Velocity is stored in the world frame, so the residual is it verbatim; the Jacobian block is $R$ because the group’s right perturbation moves it by $R\delta \mathbf{\nu }$.

ZeroAngularRateFactor — ZARU

While at rest, the gyroscope reads its own bias. This is what makes a stop observe the bias rather than merely stop accumulating it, so it complements ZUPT rather than duplicating it.

Blocks $[\text{bias}(6)]$ — residual 3, Jacobian $3\times 6$.

$$\mathbf{r}=\tilde{\mathbf{\omega }}-{\mathbf{b}}_g,\frac{\partial \mathbf{r}}{\partial \mathbf{b}}=[-I_3|0_{3\times 3}]$$

with $\tilde{\mathbf{\omega }}$ the gyro reading averaged over the stationary interval. The accelerometer half is untouched — at rest the accelerometer reads gravity, not its bias, so a stop says nothing about ${\mathbf{b}}_a$ without an attitude reference.

NonholonomicFactor

A wheeled vehicle does not slide sideways or leave the ground: in the body frame its velocity has no lateral or vertical component.

Blocks $[{\text{SE}}_2(3)\text{state}]$ — residual 2, Jacobian $2\times 9$.

$${\mathbf{v}}_{\text{body}}=R^{\top }{\mathbf{v}}_{\text{world}},\mathbf{r}=\left[\begin{array}{c}{v}_{\text{body},y}\\ v_{\text{body},z}\end{array}\right]$$

$$\frac{\partial {\mathbf{v}}_{\text{body}}}{\partial \delta \mathbf{\rho }}=0,\frac{\partial {\mathbf{v}}_{\text{body}}}{\partial \delta \mathbf{\theta }}=[{\mathbf{v}}_{\text{body}}{]}_{\times },\frac{\partial {\mathbf{v}}_{\text{body}}}{\partial \delta \mathbf{\nu }}=I$$

taking rows $y$ and $z$. The $\mathbf{\theta }$ block is what makes this informative about attitude: a heading error turns forward motion into apparent lateral slip, so the constraint corrects the heading, not just the velocity.

Two nearly-free constraints per state, and on a ground vehicle they bound the lateral drift inertial integration is worst at. They are violated by skidding, by lifting a wheel, and by any vehicle that can translate sideways.

PlanarMotionFactor

A vehicle confined to a horizontal plane: fixed height, no roll, no pitch.

Blocks $[\text{SE}(3)\text{pose}]$ — residual 3, Jacobian $3\times 6$.

$$\mathbf{r}=\left[\begin{array}{c}{t}_z-h\\ {\mathbf{e}}_z^{\top }R{\mathbf{e}}_x\\ {\mathbf{e}}_z^{\top }R{\mathbf{e}}_y\end{array}\right]=\left[\begin{array}{c}{t}_z-h\\ R_{31}\\ R_{32}\end{array}\right]$$

The two tilt rows are the world-$z$ components of the body’s $x$ and $y$ axes, not roll and pitch angles. They vanish on exactly the same set, but Euler extraction introduces a gimbal singularity and a discontinuity that a least-squares residual should not carry; these rows are smooth everywhere.

$$\frac{\partial r_0}{\partial \delta \mathbf{\rho }}={\mathbf{e}}_z^{\top }R,\frac{\partial r_1}{\partial \delta \mathbf{\theta }}=-{\mathbf{e}}_z^{\top }R[{\mathbf{e}}_x{]}_{\times },\frac{\partial r_2}{\partial \delta \mathbf{\theta }}=-{\mathbf{e}}_z^{\top }R[{\mathbf{e}}_y{]}_{\times }$$

with $\partial r_0/\partial \delta \mathbf{\theta }=0$ and $\partial r_{1,2}/\partial \delta \mathbf{\rho }=0$. The rotation blocks follow from $R\leftarrow R\mathrm{Exp}(\delta \mathbf{\theta })\approx R(I+[\delta \mathbf{\theta }{]}_{\times })$ and $R[\delta \mathbf{\theta }{]}_{\times }{\mathbf{e}}_a=-R[{\mathbf{e}}_a{]}_{\times }\delta \mathbf{\theta }$.

Three constraints per pose, removing the three least-observable degrees of freedom in a 6-DOF estimate of an indoor or road vehicle.