IMU filtering and odom fusion for a fast 4WD rover that jumps and slides

I disagree with the Madgwick then EKF idea. Madgwick is great for quick attitude demos but it throws away a proper noise model and doubles up filtering. For a jumping, sliding rover you want one filter (EKF or UKF) taking raw accel+gyro at IMU rate and treating encoders as conditional measurements gated by contact.

Practical approach: run the full filter at 200–400 Hz, feed raw IMU in, and apply encoder updates only when a contact flag is true. Detect airborne/impact: sustained vertical accel near 0g for >20 ms -> airborne; accel spikes >3 g -> impact. Detect slip by comparing IMU-integrated forward velocity vs wheel odom or wheel speed vs commanded speed; if mismatch >20% or wheels spin fast with low lateral accel, mark encoders untrusted.

When airborne or slipping zero-weight encoder updates or set encoder measurement covariance very large. On impact don’t trust instantaneous accel for velocity jumps, instead blow up process noise on velocity for 50–300 ms and let the filter recover.

Suggested starter covariances (tune on bench): gyro meas sigma ~0.01 rad/s, accel meas sigma ~0.1 m/s^2, encoder velocity meas sigma healthy ~0.05 m/s, slip sigma ~1.0 m/s. Process noise on velocity nominal ~0.1 (m/s^2 variance), on impact set to 5–10 for a short burst. Keep it simple, use a contact flag and aggressive covariance switching rather than stacking filters.

Skater here. Short practical plan:

1) Filters
- Run Madgwick at 400 Hz for attitude (beta ~0.04–0.12). Feed quaternion + gyro bias into an EKF/UKF for pose and velocity. Full raw-IMU EKF is OK if you want joint bias estimation and have CPU, but Madgwick+EKF is simpler and fast.

2) Handling slip/impacts
- Slip: compare encoder speed to IMU-derived speed or accel-integrated velocity and compare encoder yaw rate to gyro yaw. If |v_enc - v_imu| > 0.3–0.5 m/s or |yaw_enc - yaw_gyro| > 0.3 rad/s, inflate encoder measurement covariance or drop encoder updates for 0.2–1.0 s.
- Impact/airborne: detect accel magnitude spikes > 4–6 g or vertical accel change > 3–4 g; mark airborne. While airborne ignore encoders, raise process noise on velocity and yaw. On landing burst process noise for 0.1–0.5 s and re-enable encoders after validation.

3) Example noise/stiffness numbers (start here, then tune)
- Gyro std dev: 0.01–0.02 rad/s
- Accel std dev: 0.1–0.3 m/s^2
- Gyro bias RW: 1e-4–1e-3 (units of rad/s per sqrt(s))
- Encoder velocity std dev nominal: 0.02–0.1 m/s; during slip set to ~1.0 m/s
- Madgwick beta: 0.04–0.12
- EKF update: 100–200 Hz

4) Implementation tips
- Inflate encoder covariance to "zero-weight" rather than hard dropping unless needed. Use time gating after impact.
- Log lots and tune thresholds from real jumps/slides.

Want help picking encoder thresholds from your wheel diameter and gear ratio?

Two-stage is the practical choice: run Madgwick at 400 Hz for attitude and feed bias-corrected accel + orientation into a small EKF for velocity/pose with encoder speed as measurements.

Key heuristics
- Slip: wheel speed > fused forward speed + 0.5 m/s or sudden encoder RPM spike; on slip set encoder meas cov large (sigma 5 m/s) or drop encoder updates for 0.2–0.5 s.
- Landing/impact: vertical accel spike > 3–4 g or huge accel jerk; on impact inflate velocity process noise and reset vertical vel to zero, hold high cov for 0.1–0.5 s.

Suggested tuning (starting points)
- Madgwick beta: 0.03–0.08 (try 0.05 at 400 Hz).
- IMU noise cov (EKF): R_gyro ~ (0.01 rad/s)^2, R_accel ~ (0.1 m/s^2)^2.
- Encoder speed meas cov (normal): R_enc ~ (0.2 m/s)^2; when slip -> (5 m/s)^2.
- Process noise (acc drive on vel): q_acc ~ (0.5 m/s^2)^2 normally, bump to (3 m/s^2)^2 during impacts.
- Bias RW: ~1e-4 to 1e-6 (tune if you see drift).

Other tips
- Low-pass accel for slip/impact detection (cutoff ~20–40 Hz). Use small state EKF to keep CPU low on Teensy.

Reply in comments if you want help tuning these on real logs or need ROS2/robot_localization mapping.