Crate kinavis_ins 

Strapdown inertial navigation over the KINAVIS kernel.

Integrating IMU rate and specific force gives attitude, velocity and position without external aids, but sensor biases integrate too: 1°/h of gyro drift becomes about a mile of position error in half an hour. The system therefore has two parts:

1. Mechanisation, Strapdown: NED integration including Earth rate, transport rate, gravity and Coriolis.
2. Error-state filter, InsFilter: 15 states (position, velocity, attitude, gyro bias, accelerometer bias errors) estimated from available aiding — GNSS position or velocity, heading, zero-velocity — and fed back into the mechanisation to bound drift.

Outputs are kernel types: Position, a Vector3 of Speed, an Attitude with yaw as TrueCourse, plus sigmas and the error ellipse; never the covariance matrix. InsMotion is a process model for the six-state estimator in kinavis, propagating position between fixes with the INS velocity.

No allocation, no panics on any input; builds for bare-metal targets and CI checks the strict-profile build for panic paths. Filter consistency is verified by Monte Carlo tests: over a passage with a turn, NEES (15 states) and NIS of position fixes fall within their χ² intervals.

use core::time::Duration;
use kinavis_ins::{
    gravity_down, GatingPolicy, ImuNoise, ImuSample, InsFilter, InsPriors, Quaternion,
    Strapdown, EARTH_RATE,
};
use kinavis_kernel::{
    Angle, Distance, GeodeticPoint, Height, Instant, Ned, Speed, TrueCourse, Utc, Vector3,
};

// Alongside at 50°45.3'N, heading 037° by the gyrocompass, level.
let start = Instant::<Utc>::from_unix_seconds(1_789_000_000);
let berth = GeodeticPoint::new(
    "50°45.3'N 001°20.0'W".parse()?,
    Height::above_ellipsoid(Distance::ZERO),
);
let attitude = Quaternion::from_euler(Angle::ZERO, Angle::ZERO, TrueCourse::new(37.0)?);
let still = Vector3::<Ned, Speed>::new(Speed::ZERO, Speed::ZERO, Speed::ZERO);
let mut ins = InsFilter::new(
    Strapdown::new(start, berth, still, attitude)?,
    ImuNoise::mems(),
    &InsPriors::standard(),
);

// What the IMU reads at rest: the Earth turning, and minus gravity.
let latitude = 50.755_f64.to_radians();
let earth = [EARTH_RATE * latitude.cos(), 0.0, -EARTH_RATE * latitude.sin()];
let gravity = [0.0, 0.0, -gravity_down(latitude.sin(), 0.0)];
let sample = ImuSample::new(
    attitude.rotate_back(earth),
    attitude.rotate_back(gravity),
    Duration::from_millis(100),
)?;

// A second of samples; then the gyrocompass says 037.2°, and the
// mooring lines say the vessel is not moving.
for _ in 0..10 {
    ins.predict(&sample)?;
}
ins.update_heading(TrueCourse::new(37.2)?, Angle::from_degrees(0.5)?, GatingPolicy::none())?;
ins.update_zero_velocity(Speed::from_metres_per_second(0.02)?, GatingPolicy::none())?;

assert!((ins.attitude().yaw.degrees() - 37.2).abs() < 0.1);
assert!(ins.heading_sigma().degrees() < 0.5);
assert!(ins.velocity().magnitude().metres_per_second() < 0.02);

Not implemented

• Initial alignment (levelling on gravity, gyrocompassing on Earth rate): the caller supplies the initial attitude (gyrocompass and level, or a previous run) and the filter refines it.
• IMU-to-antenna lever arm.
• Coning and sculling corrections: a marine IMU at tens of hertz on a slow vessel is within tolerance without them.

Feature flags

• std (default) — standard library maths in the kernel.
• libm — for no_std targets: --no-default-features --features libm.
• serde — serialisation of the value types.

Structs

Attitude

Roll, pitch and yaw.

GatingPolicy

Innovation gating policy.

ImuNoise

IMU error model from the datasheet: white noise per channel and bias random walk.

ImuSample

One IMU sample: angular rate and specific force in the body frame, averaged over the interval since the previous sample.

InsFilter

Inertial navigation filter: a Strapdown mechanisation with its 15-state error filter.

InsMotion

INS-reported motion as a process model.

InsPriors

Initial 1σ uncertainty of each part of the state.

InsUpdate

Update result.

Quaternion

Unit quaternion [w, x, y, z], body to navigation frame.

Strapdown

Nominal INS state: position, velocity, attitude and estimated sensor biases.

Constants

EARTH_RATE

Earth rotation rate, rad/s (WGS 84).

ERROR_STATE_DIM

Error-state dimension: position, velocity, attitude, gyro bias, accelerometer bias; three each.

Functions

gravity_down

Gravity at latitude and height, m/s², positive down: Somigliana on WGS 84 with free-air correction.