Add Z-axis (altitude) estimation to EKF

[adam-weber]

Problem Statement

The current EKF estimates position and velocity in 2D only. GPS altitude accuracy is poor (typical VDOP 1.5-3x worse than horizontal), making elevation tracking unreliable for events with significant vertical changes.

Based on findings from: GPS-IMU Sensor Fusion for Reliable Autonomous Vehicle Position Estimation

Proposed Solution

Expand the EKF from 7 states to 10 states to include altitude estimation:

Current:  x = [x, y, ψ, vx, vy, bax, bay]
Proposed: x = [x, y, z, ψ, vx, vy, vz, bax, bay, baz]

Fuse three data sources for altitude:

1. Vertical acceleration from IMU
2. Barometric pressure altitude (BMP280/BMP388)
3. GPS altitude

Research on low-cost GPS-IMU sensor fusion demonstrates the value of this approach:

• GPS-only Z RMSE: 13.4m
• Fused Z RMSE: 0.2m

Use Case

• Hill climb events (Pikes Peak, rally stages with elevation change)
• Banked corner analysis
• Jump detection (rally, off-road)
• Suspension heave/pitch analysis

Alternatives Considered

• GPS altitude only: Too noisy (13m+ error typical)
• Barometer only: Drifts with weather, no absolute reference
• Post-processing altitude from maps: Requires known route, doesn't capture dynamics

Implementation Ideas

Modules affected:

• ekf.rs: Expand state vector 7→10, covariance 7x7→10x10, add vertical prediction/update
• transforms.rs: Already computes az, just need to use it
• main.rs: Add barometer initialization and reading
• binary_telemetry.rs: Add altitude field to packet

New hardware:

• BMP280 or BMP388 barometric sensor (I2C, ~$3)
• Shares I2C bus if other sensors present

Performance:

• Matrix operations increase from 7x7 to 10x10 (~2x compute for matrix ops)
• Additional I2C read per loop (~1ms)
• Memory: ~200 bytes additional for expanded matrices

Additional Context

Low priority. Current 2D implementation is sufficient for flat circuits. This enhancement is for specific use cases with elevation changes.