Primers
- Did you turn on the message debugging flag, or are you compiling with
PRINT_DEBUG_MSGS? - Are you sure you are launching the right test?
Covariance Update with $\Delta t$
In a Kalman filter, the covariance prediction step is usually written as:
\[P' = FPF^T + Q\]where $F$ propagates the current uncertainty through the system dynamics, and $Q$ adds new uncertainty from process noise.,One detail is easy to miss: the process noise must be scaled correctly by the time interval $\Delta t$.
A useful way to understand this is to start from a simple motion model:
\[\dot{p} = v, \qquad \dot{v} = a\]Suppose the acceleration contains noise:
\[a = a_{\text{true}} + n, n \sim \mathcal{N}(0, \sigma^2)\]Over a small time interval $\Delta t$, this acceleration noise affects velocity and position as:
\[\Delta v = n \Delta t , \Delta p = \frac{1}{2} n \Delta t^2\]Think of time as scaling the covariance. we can use a basic covariance rule:
$$
\begin{gather*}
&
\text{Cov}(cX) = c^2 \text{Cov}(X)
\ & \text{Cov}(\Delta v)=\sigma^2 \Delta t^2
\ & \text{Cov}(\Delta p) = \frac{1}{4}\sigma^2 \Delta t^4
\end{gather*} $$ There is also a cross-covariance between position error and velocity error, because both come from the same acceleration noise source:
\[\text{Cov}(\Delta p, \Delta v) E[\Delta p \Delta v] = \text{Cov}(\Delta p, \Delta v) E\left[ \left(\frac{1}{2}n\Delta t^2\right) \left(n\Delta t\right) \right] = \text{Cov}(\Delta p, \Delta v) \frac{1}{2}\sigma^2 \Delta t^3\]So for a simple 1D position-velocity system affected by acceleration noise, the discrete process covariance has the structure:
\[Q = \sigma^2 \begin{bmatrix} \frac{1}{4}\Delta t^4 & \frac{1}{2}\Delta t^3 \ \frac{1}{2}\Delta t^3 & \Delta t^2 \end{bmatrix}\]The same idea applies to orientation. If angular velocity contains gyro noise:
\[\omega = \omega_{\text{true}} + n_g , \Delta \theta = n_g \Delta t => \text{Cov}(\Delta \theta) \sigma_g^2 \Delta t^2\]IMU tools
Velocity drift observed. Here, I’m proposing a two-phase static test to confirm true sensor drift:
- Initialization—standstill for N seconds to estimate biases.
- Static logging—remain stationary and record velocity.
- If velocity continues to grow, it’s genuine drift.
- Initial results show the same drift pattern—next step is to connect directly to the IMU board (bypass middleware) to rule out software filtering or fusion artifacts.
I observed that angular rate remains stable over the static test; no significant gyro drift detected.
Accelerometer bias
- Sampling at 33 Hz, the mean linear-acceleration bias is ≈ 0.3 m/s² (≈ 0.03 g).
- Projected drift: ~560 m over one minute—still within typical MEMS accelerometer error bounds.
- Changing the static initialization window shifts the estimated accelerometer bias (ba) and gyro bias (bg), which alters the trajectory shape but does not eliminate the drift or induced rotation.
- Bias characteristics
- IMU biases are not fixed constants. They:
- Drift with temperature.
- Exhibit bias-random-walk (BRW) that grows proportionally to √t.
- Often follow a 1/f noise spectrum rather than pure white noise.
- Tactical in-run bias stability is on the order of micro-g, but bias repeatability is typically larger—see Advanced Navigation’s IMU introduction for details.
- IMU biases are not fixed constants. They:
GPS Debugging Notes
- When GPS heading is invalid, are you omitting GPS messages?
- One may choose not to omit GPS. We could just set their ESKF jacobians to 0.
- GPS needs IMU to be initialized
- IMU integration needs the first GPS to be found
Data synchronization
- Do you get messages exactly the same timing?
- Are data synchronization, like the GPS and IMU Data sync correct?
- When GPS starts becoming valid, do you choose the pose there to be your initial pose?
- Initial velocity is from the IMU. They are not directly corrected by GPS. So wrong initial velocity could make IMU spin a lot while its cartesian pose is around the GPS point
