Skip to main content

rust_robotics_slam/
imu_preintegration.rs

1//! Bias-aware IMU preintegration with covariance propagation.
2//!
3//! Error states use `[rotation, position, velocity]`; biases use
4//! `[accelerometer, gyroscope]`.
5
6use nalgebra::{DMatrix, DVector, Matrix3, Matrix4, SMatrix, Vector3};
7use rust_robotics_core::{skew, so3_exp, so3_left_jacobian, so3_left_jacobian_inverse, so3_log};
8use rust_robotics_optimization::{
9    solve, Factor, FactorEvaluation, OptimizationError, OptimizationResult, Problem, SolverConfig,
10    SolverSummary, Variable, VariableId,
11};
12
13pub type Matrix9 = SMatrix<f64, 9, 9>;
14pub type Matrix9x6 = SMatrix<f64, 9, 6>;
15pub type Matrix6 = SMatrix<f64, 6, 6>;
16
17/// One accelerometer/gyroscope sample expressed in an IMU sensor frame.
18#[derive(Debug, Clone, Copy, PartialEq)]
19pub struct ImuMeasurement {
20    pub acceleration: Vector3<f64>,
21    pub angular_velocity: Vector3<f64>,
22}
23
24/// Rigid mounting transform from an IMU sensor frame to the navigation body frame.
25#[derive(Debug, Clone, Copy, PartialEq)]
26pub struct ImuExtrinsics {
27    pub rotation_body_from_sensor: Matrix3<f64>,
28    pub translation_body_from_sensor: Vector3<f64>,
29}
30
31impl ImuExtrinsics {
32    pub fn identity() -> Self {
33        Self {
34            rotation_body_from_sensor: Matrix3::identity(),
35            translation_body_from_sensor: Vector3::zeros(),
36        }
37    }
38
39    /// Extracts the rotation and translation from a homogeneous body-from-sensor transform.
40    pub fn from_homogeneous(body_from_sensor: &Matrix4<f64>) -> OptimizationResult<Self> {
41        if !body_from_sensor.iter().all(|value| value.is_finite()) {
42            return Err(OptimizationError::InvalidParameter(
43                "IMU extrinsic transform must be finite".into(),
44            ));
45        }
46        let bottom_row = body_from_sensor.row(3);
47        if (bottom_row[0].abs() > 1.0e-12)
48            || (bottom_row[1].abs() > 1.0e-12)
49            || (bottom_row[2].abs() > 1.0e-12)
50            || ((bottom_row[3] - 1.0).abs() > 1.0e-12)
51        {
52            return Err(OptimizationError::InvalidParameter(
53                "IMU extrinsic transform must be homogeneous".into(),
54            ));
55        }
56        let rotation = body_from_sensor.fixed_view::<3, 3>(0, 0).into_owned();
57        let orthogonality_error = rotation.transpose() * rotation - Matrix3::identity();
58        if orthogonality_error.norm() > 1.0e-8 || (rotation.determinant() - 1.0).abs() > 1.0e-8 {
59            return Err(OptimizationError::InvalidParameter(
60                "IMU extrinsic rotation must be in SO(3)".into(),
61            ));
62        }
63        Ok(Self {
64            rotation_body_from_sensor: rotation,
65            translation_body_from_sensor: body_from_sensor.fixed_view::<3, 1>(0, 3).into_owned(),
66        })
67    }
68
69    /// Transforms a sensor-frame IMU sample to the body frame.
70    ///
71    /// The acceleration includes centripetal and tangential lever-arm terms,
72    /// matching MathematicalRobotics `transformIMU`.
73    pub fn transform(
74        &self,
75        measurement: ImuMeasurement,
76        angular_acceleration_sensor: Vector3<f64>,
77    ) -> ImuMeasurement {
78        let angular_velocity = self.rotation_body_from_sensor * measurement.angular_velocity;
79        let angular_acceleration = self.rotation_body_from_sensor * angular_acceleration_sensor;
80        let translation = self.translation_body_from_sensor;
81        let acceleration = self.rotation_body_from_sensor * measurement.acceleration
82            - skew(&angular_velocity) * skew(&angular_velocity) * translation
83            + skew(&translation) * angular_acceleration;
84        ImuMeasurement {
85            acceleration,
86            angular_velocity,
87        }
88    }
89}
90
91/// Accelerometer and gyroscope biases.
92#[derive(Debug, Clone, Copy, PartialEq)]
93pub struct ImuBias {
94    pub accelerometer: Vector3<f64>,
95    pub gyroscope: Vector3<f64>,
96}
97
98impl ImuBias {
99    pub fn zero() -> Self {
100        Self {
101            accelerometer: Vector3::zeros(),
102            gyroscope: Vector3::zeros(),
103        }
104    }
105
106    fn vector(&self) -> DVector<f64> {
107        DVector::from_vec(vec![
108            self.accelerometer.x,
109            self.accelerometer.y,
110            self.accelerometer.z,
111            self.gyroscope.x,
112            self.gyroscope.y,
113            self.gyroscope.z,
114        ])
115    }
116}
117
118/// Navigation state in a world frame.
119#[derive(Debug, Clone, PartialEq)]
120pub struct NavState {
121    pub rotation: Matrix3<f64>,
122    pub position: Vector3<f64>,
123    pub velocity: Vector3<f64>,
124}
125
126impl NavState {
127    pub fn identity() -> Self {
128        Self {
129            rotation: Matrix3::identity(),
130            position: Vector3::zeros(),
131            velocity: Vector3::zeros(),
132        }
133    }
134}
135
136/// White measurement noise used during covariance propagation.
137#[derive(Debug, Clone, Copy, PartialEq)]
138pub struct ImuNoise {
139    pub accelerometer_sigma: f64,
140    pub gyroscope_sigma: f64,
141}
142
143impl Default for ImuNoise {
144    fn default() -> Self {
145        Self {
146            accelerometer_sigma: 0.1,
147            gyroscope_sigma: 0.01,
148        }
149    }
150}
151
152/// Preintegrated measurements between two navigation states.
153#[derive(Debug, Clone)]
154pub struct PreintegratedImuMeasurement {
155    pub delta_rotation: Matrix3<f64>,
156    pub delta_position: Vector3<f64>,
157    pub delta_velocity: Vector3<f64>,
158    pub delta_time: f64,
159    pub covariance: Matrix9,
160    pub bias_jacobian: Matrix9x6,
161    pub linearization_bias: ImuBias,
162    noise: ImuNoise,
163}
164
165impl PreintegratedImuMeasurement {
166    pub fn new(linearization_bias: ImuBias, noise: ImuNoise) -> Self {
167        Self {
168            delta_rotation: Matrix3::identity(),
169            delta_position: Vector3::zeros(),
170            delta_velocity: Vector3::zeros(),
171            delta_time: 0.0,
172            covariance: Matrix9::zeros(),
173            bias_jacobian: Matrix9x6::zeros(),
174            linearization_bias,
175            noise,
176        }
177    }
178
179    /// Integrates one body-frame specific-force and angular-rate sample.
180    pub fn integrate(
181        &mut self,
182        acceleration: Vector3<f64>,
183        angular_velocity: Vector3<f64>,
184        dt: f64,
185    ) -> OptimizationResult<()> {
186        if !dt.is_finite() || dt <= 0.0 {
187            return Err(OptimizationError::InvalidParameter(
188                "IMU sample dt must be finite and positive".into(),
189            ));
190        }
191        let acceleration = acceleration - self.linearization_bias.accelerometer;
192        let angular_velocity = angular_velocity - self.linearization_bias.gyroscope;
193        let rotation = self.delta_rotation;
194        let rotated_acceleration = rotation * acceleration;
195        let half_dt_squared = 0.5 * dt * dt;
196
197        self.delta_position += self.delta_velocity * dt + rotated_acceleration * half_dt_squared;
198        self.delta_velocity += rotated_acceleration * dt;
199        self.delta_rotation *= so3_exp(&(angular_velocity * dt));
200        self.delta_time += dt;
201
202        let mut transition = Matrix9::identity();
203        transition
204            .fixed_view_mut::<3, 3>(0, 0)
205            .copy_from(&so3_exp(&(-angular_velocity * dt)));
206        transition
207            .fixed_view_mut::<3, 3>(3, 0)
208            .copy_from(&(-rotation * skew(&acceleration) * half_dt_squared));
209        transition
210            .fixed_view_mut::<3, 3>(3, 6)
211            .copy_from(&(Matrix3::identity() * dt));
212        transition
213            .fixed_view_mut::<3, 3>(6, 0)
214            .copy_from(&(-rotation * skew(&acceleration) * dt));
215
216        let mut noise_jacobian = Matrix9x6::zeros();
217        noise_jacobian
218            .fixed_view_mut::<3, 3>(0, 3)
219            .copy_from(&(Matrix3::identity() * dt));
220        noise_jacobian
221            .fixed_view_mut::<3, 3>(3, 0)
222            .copy_from(&(rotation * half_dt_squared));
223        noise_jacobian
224            .fixed_view_mut::<3, 3>(6, 0)
225            .copy_from(&(rotation * dt));
226
227        let mut measurement_covariance = Matrix6::zeros();
228        measurement_covariance
229            .fixed_view_mut::<3, 3>(0, 0)
230            .copy_from(&(Matrix3::identity() * self.noise.accelerometer_sigma.powi(2)));
231        measurement_covariance
232            .fixed_view_mut::<3, 3>(3, 3)
233            .copy_from(&(Matrix3::identity() * self.noise.gyroscope_sigma.powi(2)));
234
235        self.covariance = transition * self.covariance * transition.transpose()
236            + noise_jacobian * measurement_covariance * noise_jacobian.transpose();
237        self.bias_jacobian = transition * self.bias_jacobian - noise_jacobian;
238        Ok(())
239    }
240
241    /// Transforms and integrates one sensor-frame IMU sample.
242    pub fn integrate_sensor_measurement(
243        &mut self,
244        measurement: ImuMeasurement,
245        angular_acceleration_sensor: Vector3<f64>,
246        extrinsics: &ImuExtrinsics,
247        dt: f64,
248    ) -> OptimizationResult<()> {
249        let body_measurement = extrinsics.transform(measurement, angular_acceleration_sensor);
250        self.integrate(
251            body_measurement.acceleration,
252            body_measurement.angular_velocity,
253            dt,
254        )
255    }
256
257    /// Predicts the next navigation state, applying first-order bias correction.
258    pub fn predict(&self, state: &NavState, bias: &ImuBias, gravity: Vector3<f64>) -> NavState {
259        let (corrected_rotation, corrected_position, corrected_velocity) =
260            self.corrected_delta(bias);
261        let dt = self.delta_time;
262
263        NavState {
264            rotation: state.rotation * corrected_rotation,
265            position: state.position
266                + state.velocity * dt
267                + gravity * (0.5 * dt * dt)
268                + state.rotation * corrected_position,
269            velocity: state.velocity + gravity * dt + state.rotation * corrected_velocity,
270        }
271    }
272
273    fn corrected_delta(&self, bias: &ImuBias) -> (Matrix3<f64>, Vector3<f64>, Vector3<f64>) {
274        let bias_delta = bias.vector() - self.linearization_bias.vector();
275        let correction = self.bias_jacobian * bias_delta;
276        (
277            self.delta_rotation * so3_exp(&Vector3::from(correction.fixed_rows::<3>(0))),
278            self.delta_position + Vector3::from(correction.fixed_rows::<3>(3)),
279            self.delta_velocity + Vector3::from(correction.fixed_rows::<3>(6)),
280        )
281    }
282}
283
284/// Relative rotation, position and velocity measurement between navigation states.
285#[derive(Debug, Clone, PartialEq)]
286pub struct NavStateDelta {
287    pub rotation: Matrix3<f64>,
288    pub position: Vector3<f64>,
289    pub velocity: Vector3<f64>,
290}
291
292impl NavStateDelta {
293    pub fn identity() -> Self {
294        Self {
295            rotation: Matrix3::identity(),
296            position: Vector3::zeros(),
297            velocity: Vector3::zeros(),
298        }
299    }
300}
301
302/// Creates a 9-DOF manifold variable for a navigation state.
303pub fn nav_state_variable(state: &NavState) -> Variable {
304    Variable::with_retraction(encode_nav_state(state), 9, retract_nav_state)
305        .expect("navigation state dimensions are valid")
306}
307
308/// Creates a Euclidean six-dimensional bias variable.
309pub fn imu_bias_variable(bias: &ImuBias) -> Variable {
310    Variable::euclidean(bias.vector())
311}
312
313/// Prior factor for one six-dimensional accelerometer/gyroscope bias.
314pub struct BiasPriorFactor {
315    variables: [VariableId; 1],
316    measurement: ImuBias,
317    information: Matrix6,
318}
319
320impl BiasPriorFactor {
321    pub fn new(variable: VariableId, measurement: ImuBias, information: Matrix6) -> Self {
322        Self {
323            variables: [variable],
324            measurement,
325            information,
326        }
327    }
328}
329
330impl Factor for BiasPriorFactor {
331    fn variable_ids(&self) -> &[VariableId] {
332        &self.variables
333    }
334
335    fn evaluate(&self, values: &[&DVector<f64>]) -> OptimizationResult<FactorEvaluation> {
336        expect_dimensions(values, &[6], "bias prior")?;
337        Ok(FactorEvaluation {
338            residual: values[0] - self.measurement.vector(),
339            information: fixed_to_dynamic(&self.information),
340            jacobians: vec![DMatrix::identity(6, 6)],
341        })
342    }
343}
344
345/// Random-walk factor constraining two consecutive IMU biases.
346pub struct BiasBetweenFactor {
347    variables: [VariableId; 2],
348    information: Matrix6,
349}
350
351impl BiasBetweenFactor {
352    pub fn new(bias_i: VariableId, bias_j: VariableId, information: Matrix6) -> Self {
353        Self {
354            variables: [bias_i, bias_j],
355            information,
356        }
357    }
358}
359
360impl Factor for BiasBetweenFactor {
361    fn variable_ids(&self) -> &[VariableId] {
362        &self.variables
363    }
364
365    fn evaluate(&self, values: &[&DVector<f64>]) -> OptimizationResult<FactorEvaluation> {
366        expect_dimensions(values, &[6, 6], "bias between")?;
367        Ok(FactorEvaluation {
368            residual: values[0] - values[1],
369            information: fixed_to_dynamic(&self.information),
370            jacobians: vec![DMatrix::identity(6, 6), -DMatrix::identity(6, 6)],
371        })
372    }
373}
374
375/// Prior factor for a full navigation state.
376pub struct NavStatePriorFactor {
377    variables: [VariableId; 1],
378    measurement: NavState,
379    information: Matrix9,
380}
381
382impl NavStatePriorFactor {
383    pub fn new(variable: VariableId, measurement: NavState, information: Matrix9) -> Self {
384        Self {
385            variables: [variable],
386            measurement,
387            information,
388        }
389    }
390
391    fn residual(&self, value: &DVector<f64>) -> nalgebra::SVector<f64, 9> {
392        let state = decode_nav_state(value);
393        nav_state_local(&state, &self.measurement)
394    }
395}
396
397impl Factor for NavStatePriorFactor {
398    fn variable_ids(&self) -> &[VariableId] {
399        &self.variables
400    }
401
402    fn evaluate(&self, values: &[&DVector<f64>]) -> OptimizationResult<FactorEvaluation> {
403        expect_dimensions(values, &[9], "navigation-state prior")?;
404        let state = decode_nav_state(values[0]);
405        let residual = self.residual(values[0]);
406        let rotation_residual = Vector3::from(residual.fixed_rows::<3>(0));
407        let local_position = Vector3::from(residual.fixed_rows::<3>(3));
408        let local_velocity = Vector3::from(residual.fixed_rows::<3>(6));
409        let inverse_rotation = state.rotation.transpose();
410        let mut jacobian = DMatrix::zeros(9, 9);
411        jacobian
412            .view_mut((0, 0), (3, 3))
413            .copy_from(&(-so3_left_jacobian_inverse(&rotation_residual)));
414        jacobian
415            .view_mut((3, 0), (3, 3))
416            .copy_from(&skew(&local_position));
417        jacobian
418            .view_mut((3, 3), (3, 3))
419            .copy_from(&(-inverse_rotation));
420        jacobian
421            .view_mut((6, 0), (3, 3))
422            .copy_from(&skew(&local_velocity));
423        jacobian
424            .view_mut((6, 6), (3, 3))
425            .copy_from(&(-inverse_rotation));
426        Ok(FactorEvaluation {
427            residual: DVector::from_column_slice(residual.as_slice()),
428            information: fixed_to_dynamic(&self.information),
429            jacobians: vec![jacobian],
430        })
431    }
432}
433
434/// Constrains velocity to the finite difference between consecutive positions.
435pub struct PositionVelocityFactor {
436    variables: [VariableId; 2],
437    delta_time: f64,
438    information: Matrix3<f64>,
439}
440
441impl PositionVelocityFactor {
442    pub fn new(
443        state_i: VariableId,
444        state_j: VariableId,
445        delta_time: f64,
446        information: Matrix3<f64>,
447    ) -> OptimizationResult<Self> {
448        if !delta_time.is_finite() || delta_time <= 0.0 {
449            return Err(OptimizationError::InvalidParameter(
450                "position/velocity factor dt must be finite and positive".into(),
451            ));
452        }
453        Ok(Self {
454            variables: [state_i, state_j],
455            delta_time,
456            information,
457        })
458    }
459}
460
461impl Factor for PositionVelocityFactor {
462    fn variable_ids(&self) -> &[VariableId] {
463        &self.variables
464    }
465
466    fn evaluate(&self, values: &[&DVector<f64>]) -> OptimizationResult<FactorEvaluation> {
467        expect_dimensions(values, &[9, 9], "position/velocity")?;
468        let state_i = decode_nav_state(values[0]);
469        let state_j = decode_nav_state(values[1]);
470        let inverse_dt = self.delta_time.recip();
471        let residual = state_i.velocity - (state_j.position - state_i.position) * inverse_dt;
472        let mut jacobian_i = DMatrix::zeros(3, 9);
473        jacobian_i
474            .view_mut((0, 3), (3, 3))
475            .copy_from(&(Matrix3::identity() * inverse_dt));
476        jacobian_i
477            .view_mut((0, 6), (3, 3))
478            .copy_from(&Matrix3::identity());
479        let mut jacobian_j = DMatrix::zeros(3, 9);
480        jacobian_j
481            .view_mut((0, 3), (3, 3))
482            .copy_from(&(-Matrix3::identity() * inverse_dt));
483        Ok(FactorEvaluation {
484            residual: DVector::from_column_slice(residual.as_slice()),
485            information: fixed_to_dynamic(&self.information),
486            jacobians: vec![jacobian_i, jacobian_j],
487        })
488    }
489}
490
491/// Relative navigation-state factor, equivalent to MathematicalRobotics `NavitransEdge`.
492pub struct NavStateBetweenFactor {
493    variables: [VariableId; 2],
494    measurement: NavStateDelta,
495    information: Matrix9,
496}
497
498impl NavStateBetweenFactor {
499    pub fn new(
500        state_i: VariableId,
501        state_j: VariableId,
502        measurement: NavStateDelta,
503        information: Matrix9,
504    ) -> Self {
505        Self {
506            variables: [state_i, state_j],
507            measurement,
508            information,
509        }
510    }
511
512    fn residual(
513        &self,
514        value_i: &DVector<f64>,
515        value_j: &DVector<f64>,
516    ) -> nalgebra::SVector<f64, 9> {
517        let state_i = decode_nav_state(value_i);
518        let state_j = decode_nav_state(value_j);
519        let predicted = nav_state_delta(&state_i, &state_j);
520        let rotation_error = so3_log(&(self.measurement.rotation.transpose() * predicted.rotation));
521        nalgebra::SVector::<f64, 9>::from_iterator(
522            rotation_error
523                .iter()
524                .chain((predicted.position - self.measurement.position).iter())
525                .chain((predicted.velocity - self.measurement.velocity).iter())
526                .copied(),
527        )
528    }
529}
530
531impl Factor for NavStateBetweenFactor {
532    fn variable_ids(&self) -> &[VariableId] {
533        &self.variables
534    }
535
536    fn evaluate(&self, values: &[&DVector<f64>]) -> OptimizationResult<FactorEvaluation> {
537        expect_dimensions(values, &[9, 9], "navigation-state between")?;
538        let state_i = decode_nav_state(values[0]);
539        let state_j = decode_nav_state(values[1]);
540        let residual = self.residual(values[0], values[1]);
541        let local_position = state_i.rotation.transpose() * (state_j.position - state_i.position);
542        let local_velocity = state_i.rotation.transpose() * (state_j.velocity - state_i.velocity);
543        let rotation_residual = Vector3::from(residual.fixed_rows::<3>(0));
544        let inverse_rotation_i = state_i.rotation.transpose();
545        let mut jacobian_i = DMatrix::zeros(9, 9);
546        jacobian_i.view_mut((0, 0), (3, 3)).copy_from(
547            &(-so3_left_jacobian_inverse(&rotation_residual)
548                * self.measurement.rotation.transpose()),
549        );
550        jacobian_i
551            .view_mut((3, 0), (3, 3))
552            .copy_from(&skew(&local_position));
553        jacobian_i
554            .view_mut((3, 3), (3, 3))
555            .copy_from(&(-inverse_rotation_i));
556        jacobian_i
557            .view_mut((6, 0), (3, 3))
558            .copy_from(&skew(&local_velocity));
559        jacobian_i
560            .view_mut((6, 6), (3, 3))
561            .copy_from(&(-inverse_rotation_i));
562
563        let mut jacobian_j = DMatrix::zeros(9, 9);
564        jacobian_j
565            .view_mut((0, 0), (3, 3))
566            .copy_from(&so3_left_jacobian_inverse(&(-rotation_residual)));
567        jacobian_j
568            .view_mut((3, 3), (3, 3))
569            .copy_from(&inverse_rotation_i);
570        jacobian_j
571            .view_mut((6, 6), (3, 3))
572            .copy_from(&inverse_rotation_i);
573        Ok(FactorEvaluation {
574            residual: DVector::from_column_slice(residual.as_slice()),
575            information: fixed_to_dynamic(&self.information),
576            jacobians: vec![jacobian_i, jacobian_j],
577        })
578    }
579}
580
581/// IMU factor linking `(state_i, state_j, bias_i)`.
582pub struct ImuFactor {
583    variables: [VariableId; 3],
584    measurement: PreintegratedImuMeasurement,
585    gravity: Vector3<f64>,
586}
587
588impl ImuFactor {
589    pub fn new(
590        state_i: VariableId,
591        state_j: VariableId,
592        bias_i: VariableId,
593        measurement: PreintegratedImuMeasurement,
594        gravity: Vector3<f64>,
595    ) -> Self {
596        Self {
597            variables: [state_i, state_j, bias_i],
598            measurement,
599            gravity,
600        }
601    }
602}
603
604impl Factor for ImuFactor {
605    fn variable_ids(&self) -> &[VariableId] {
606        &self.variables
607    }
608
609    fn evaluate(&self, values: &[&DVector<f64>]) -> OptimizationResult<FactorEvaluation> {
610        if values[0].len() != 9 || values[1].len() != 9 || values[2].len() != 6 {
611            return Err(OptimizationError::InvalidFactor(
612                "IMU factor expects state(9), state(9), bias(6)".into(),
613            ));
614        }
615        let residual = self.residual(values);
616        let jacobians = self.analytic_jacobians(values, &residual);
617        let regularized = self.measurement.covariance + Matrix9::identity() * 1.0e-12;
618        let information = regularized
619            .try_inverse()
620            .ok_or(OptimizationError::LinearSolveFailed)?;
621        Ok(FactorEvaluation {
622            residual: DVector::from_column_slice(residual.as_slice()),
623            information: DMatrix::from_column_slice(9, 9, information.as_slice()),
624            jacobians,
625        })
626    }
627}
628
629impl ImuFactor {
630    fn residual(&self, values: &[&DVector<f64>]) -> nalgebra::SVector<f64, 9> {
631        let state_i = decode_nav_state(values[0]);
632        let state_j = decode_nav_state(values[1]);
633        let bias = decode_bias(values[2]);
634        let (delta_rotation, delta_position, delta_velocity) =
635            self.measurement.corrected_delta(&bias);
636        let dt = self.measurement.delta_time;
637        let rotation_error = so3_log(
638            &(delta_rotation.transpose() * state_i.rotation.transpose() * state_j.rotation),
639        );
640        let position_error = state_i.rotation.transpose()
641            * (state_j.position
642                - state_i.position
643                - state_i.velocity * dt
644                - self.gravity * (0.5 * dt * dt))
645            - delta_position;
646        let velocity_error = state_i.rotation.transpose()
647            * (state_j.velocity - state_i.velocity - self.gravity * dt)
648            - delta_velocity;
649        nalgebra::SVector::<f64, 9>::from_iterator(
650            rotation_error
651                .iter()
652                .chain(position_error.iter())
653                .chain(velocity_error.iter())
654                .copied(),
655        )
656    }
657
658    fn analytic_jacobians(
659        &self,
660        values: &[&DVector<f64>],
661        residual: &nalgebra::SVector<f64, 9>,
662    ) -> Vec<DMatrix<f64>> {
663        let state_i = decode_nav_state(values[0]);
664        let state_j = decode_nav_state(values[1]);
665        let bias = decode_bias(values[2]);
666        let inverse_rotation_i = state_i.rotation.transpose();
667        let dt = self.measurement.delta_time;
668        let position_delta = state_j.position
669            - state_i.position
670            - state_i.velocity * dt
671            - self.gravity * (0.5 * dt * dt);
672        let velocity_delta = state_j.velocity - state_i.velocity - self.gravity * dt;
673        let local_position = inverse_rotation_i * position_delta;
674        let local_velocity = inverse_rotation_i * velocity_delta;
675        let rotation_residual = Vector3::from(residual.fixed_rows::<3>(0));
676        let left_inverse = so3_left_jacobian_inverse(&rotation_residual);
677        let right_inverse = so3_left_jacobian_inverse(&(-rotation_residual));
678        let (corrected_rotation, _, _) = self.measurement.corrected_delta(&bias);
679
680        let mut state_i_jacobian = DMatrix::zeros(9, 9);
681        state_i_jacobian
682            .view_mut((0, 0), (3, 3))
683            .copy_from(&(-left_inverse * corrected_rotation.transpose()));
684        state_i_jacobian
685            .view_mut((3, 0), (3, 3))
686            .copy_from(&skew(&local_position));
687        state_i_jacobian
688            .view_mut((3, 3), (3, 3))
689            .copy_from(&(-inverse_rotation_i));
690        state_i_jacobian
691            .view_mut((3, 6), (3, 3))
692            .copy_from(&(-inverse_rotation_i * dt));
693        state_i_jacobian
694            .view_mut((6, 0), (3, 3))
695            .copy_from(&skew(&local_velocity));
696        state_i_jacobian
697            .view_mut((6, 6), (3, 3))
698            .copy_from(&(-inverse_rotation_i));
699
700        let mut state_j_jacobian = DMatrix::zeros(9, 9);
701        state_j_jacobian
702            .view_mut((0, 0), (3, 3))
703            .copy_from(&right_inverse);
704        state_j_jacobian
705            .view_mut((3, 3), (3, 3))
706            .copy_from(&inverse_rotation_i);
707        state_j_jacobian
708            .view_mut((6, 6), (3, 3))
709            .copy_from(&inverse_rotation_i);
710
711        let bias_delta = bias.vector() - self.measurement.linearization_bias.vector();
712        let correction = self.measurement.bias_jacobian * bias_delta;
713        let rotation_correction = Vector3::from(correction.fixed_rows::<3>(0));
714        let correction_right_jacobian = so3_left_jacobian(&(-rotation_correction));
715        let mut bias_jacobian = DMatrix::zeros(9, 6);
716        bias_jacobian.view_mut((0, 0), (3, 6)).copy_from(
717            &(-left_inverse
718                * correction_right_jacobian
719                * self.measurement.bias_jacobian.fixed_rows::<3>(0)),
720        );
721        bias_jacobian
722            .view_mut((3, 0), (3, 6))
723            .copy_from(&(-self.measurement.bias_jacobian.fixed_rows::<3>(3)));
724        bias_jacobian
725            .view_mut((6, 0), (3, 6))
726            .copy_from(&(-self.measurement.bias_jacobian.fixed_rows::<3>(6)));
727
728        vec![state_i_jacobian, state_j_jacobian, bias_jacobian]
729    }
730
731    #[cfg(test)]
732    fn numerical_jacobian(&self, values: &[&DVector<f64>], variable: usize) -> DMatrix<f64> {
733        let dimension = if variable == 2 { 6 } else { 9 };
734        let epsilon = 1.0e-6;
735        let mut jacobian = DMatrix::zeros(9, dimension);
736        for column in 0..dimension {
737            let mut increment = DVector::zeros(dimension);
738            increment[column] = epsilon;
739            let plus_value = if variable == 2 {
740                values[variable] + &increment
741            } else {
742                retract_nav_state(values[variable], &increment).expect("dimensions are valid")
743            };
744            let minus_value = if variable == 2 {
745                values[variable] - &increment
746            } else {
747                retract_nav_state(values[variable], &(-&increment)).expect("dimensions are valid")
748            };
749            let mut plus_values = values.to_vec();
750            let mut minus_values = values.to_vec();
751            plus_values[variable] = &plus_value;
752            minus_values[variable] = &minus_value;
753            let derivative =
754                (self.residual(&plus_values) - self.residual(&minus_values)) / (2.0 * epsilon);
755            jacobian.set_column(column, &derivative);
756        }
757        jacobian
758    }
759}
760
761/// Configuration for joint navigation-state and IMU-bias refinement.
762#[derive(Debug, Clone)]
763pub struct ImuTrajectoryConfig {
764    pub solver: SolverConfig,
765    pub nav_prior_information: Matrix9,
766    /// Optional information applied to every supplied initial navigation state.
767    ///
768    /// This lets visual pose estimates constrain the inertial trajectory. Zero
769    /// rows/columns can leave velocity or another component unconstrained.
770    pub nav_measurement_information: Option<Matrix9>,
771    pub bias_prior_information: Matrix6,
772    pub bias_between_information: Matrix6,
773}
774
775impl Default for ImuTrajectoryConfig {
776    fn default() -> Self {
777        Self {
778            solver: SolverConfig::default(),
779            nav_prior_information: Matrix9::identity() * 1.0e8,
780            nav_measurement_information: None,
781            bias_prior_information: Matrix6::identity() * 1.0e4,
782            bias_between_information: Matrix6::identity() * 1.0e3,
783        }
784    }
785}
786
787/// Refined navigation states, time-varying biases and solver diagnostics.
788#[derive(Debug, Clone)]
789pub struct ImuTrajectoryResult {
790    pub states: Vec<NavState>,
791    pub biases: Vec<ImuBias>,
792    pub summary: SolverSummary,
793}
794
795/// Jointly refines a navigation-state sequence and one bias per keyframe.
796///
797/// The first state and bias receive priors, each preintegrated interval adds an
798/// [`ImuFactor`], and consecutive biases receive a random-walk constraint.
799pub fn optimize_imu_trajectory(
800    initial_states: &[NavState],
801    initial_bias: ImuBias,
802    measurements: &[PreintegratedImuMeasurement],
803    gravity: Vector3<f64>,
804    config: &ImuTrajectoryConfig,
805) -> OptimizationResult<ImuTrajectoryResult> {
806    if initial_states.len() < 2 || measurements.len() + 1 != initial_states.len() {
807        return Err(OptimizationError::InvalidParameter(
808            "IMU trajectory requires N states and N-1 measurements".into(),
809        ));
810    }
811    let mut problem = Problem::new();
812    let state_ids = initial_states
813        .iter()
814        .map(|state| problem.add_variable(nav_state_variable(state)))
815        .collect::<Vec<_>>();
816    let bias_ids = initial_states
817        .iter()
818        .map(|_| problem.add_variable(imu_bias_variable(&initial_bias)))
819        .collect::<Vec<_>>();
820
821    problem.add_factor(NavStatePriorFactor::new(
822        state_ids[0],
823        initial_states[0].clone(),
824        config.nav_prior_information,
825    ))?;
826    problem.add_factor(BiasPriorFactor::new(
827        bias_ids[0],
828        initial_bias,
829        config.bias_prior_information,
830    ))?;
831    if let Some(information) = config.nav_measurement_information {
832        for (id, measurement) in state_ids.iter().zip(initial_states) {
833            problem.add_factor(NavStatePriorFactor::new(
834                *id,
835                measurement.clone(),
836                information,
837            ))?;
838        }
839    }
840    for (index, measurement) in measurements.iter().enumerate() {
841        problem.add_factor(ImuFactor::new(
842            state_ids[index],
843            state_ids[index + 1],
844            bias_ids[index],
845            measurement.clone(),
846            gravity,
847        ))?;
848        problem.add_factor(BiasBetweenFactor::new(
849            bias_ids[index],
850            bias_ids[index + 1],
851            config.bias_between_information,
852        ))?;
853    }
854
855    let summary = solve(&mut problem, &config.solver)?;
856    let states = state_ids
857        .iter()
858        .map(|id| {
859            problem
860                .variable(*id)
861                .map(|variable| decode_nav_state(variable.value()))
862        })
863        .collect::<OptimizationResult<Vec<_>>>()?;
864    let biases = bias_ids
865        .iter()
866        .map(|id| {
867            problem
868                .variable(*id)
869                .map(|variable| decode_bias(variable.value()))
870        })
871        .collect::<OptimizationResult<Vec<_>>>()?;
872    Ok(ImuTrajectoryResult {
873        states,
874        biases,
875        summary,
876    })
877}
878
879fn nav_state_delta(from: &NavState, to: &NavState) -> NavStateDelta {
880    let inverse_rotation = from.rotation.transpose();
881    NavStateDelta {
882        rotation: inverse_rotation * to.rotation,
883        position: inverse_rotation * (to.position - from.position),
884        velocity: inverse_rotation * (to.velocity - from.velocity),
885    }
886}
887
888fn nav_state_local(from: &NavState, to: &NavState) -> nalgebra::SVector<f64, 9> {
889    let delta = nav_state_delta(from, to);
890    let rotation = so3_log(&delta.rotation);
891    nalgebra::SVector::<f64, 9>::from_iterator(
892        rotation
893            .iter()
894            .chain(delta.position.iter())
895            .chain(delta.velocity.iter())
896            .copied(),
897    )
898}
899
900fn expect_dimensions(
901    values: &[&DVector<f64>],
902    dimensions: &[usize],
903    factor_name: &str,
904) -> OptimizationResult<()> {
905    if values.len() != dimensions.len()
906        || values
907            .iter()
908            .zip(dimensions)
909            .any(|(value, dimension)| value.len() != *dimension)
910    {
911        return Err(OptimizationError::InvalidFactor(format!(
912            "{factor_name} factor received invalid variable dimensions"
913        )));
914    }
915    Ok(())
916}
917
918fn fixed_to_dynamic<const N: usize>(matrix: &SMatrix<f64, N, N>) -> DMatrix<f64> {
919    DMatrix::from_column_slice(N, N, matrix.as_slice())
920}
921
922fn encode_nav_state(state: &NavState) -> DVector<f64> {
923    let rotation = so3_log(&state.rotation);
924    DVector::from_vec(vec![
925        rotation.x,
926        rotation.y,
927        rotation.z,
928        state.position.x,
929        state.position.y,
930        state.position.z,
931        state.velocity.x,
932        state.velocity.y,
933        state.velocity.z,
934    ])
935}
936
937fn decode_nav_state(value: &DVector<f64>) -> NavState {
938    NavState {
939        rotation: so3_exp(&Vector3::new(value[0], value[1], value[2])),
940        position: Vector3::new(value[3], value[4], value[5]),
941        velocity: Vector3::new(value[6], value[7], value[8]),
942    }
943}
944
945fn decode_bias(value: &DVector<f64>) -> ImuBias {
946    ImuBias {
947        accelerometer: Vector3::new(value[0], value[1], value[2]),
948        gyroscope: Vector3::new(value[3], value[4], value[5]),
949    }
950}
951
952fn retract_nav_state(
953    value: &DVector<f64>,
954    increment: &DVector<f64>,
955) -> OptimizationResult<DVector<f64>> {
956    if value.len() != 9 || increment.len() != 9 {
957        return Err(OptimizationError::InvalidParameter(
958            "navigation state and increment must have dimension 9".into(),
959        ));
960    }
961    let state = decode_nav_state(value);
962    let updated = NavState {
963        rotation: state.rotation * so3_exp(&Vector3::new(increment[0], increment[1], increment[2])),
964        position: state.position + Vector3::new(increment[3], increment[4], increment[5]),
965        velocity: state.velocity + Vector3::new(increment[6], increment[7], increment[8]),
966    };
967    Ok(encode_nav_state(&updated))
968}
969
970#[cfg(test)]
971mod tests {
972    use rust_robotics_optimization::{Factor, Problem};
973
974    use super::*;
975
976    fn numerical_factor_jacobian(
977        factor: &dyn Factor,
978        values: &[DVector<f64>],
979        variable: usize,
980        nav_state: bool,
981    ) -> DMatrix<f64> {
982        let residual_dimension = {
983            let refs = values.iter().collect::<Vec<_>>();
984            factor.evaluate(&refs).unwrap().residual.len()
985        };
986        let dimension = values[variable].len();
987        let epsilon = 1.0e-6;
988        let mut jacobian = DMatrix::zeros(residual_dimension, dimension);
989        for column in 0..dimension {
990            let mut increment = DVector::zeros(dimension);
991            increment[column] = epsilon;
992            let mut plus = values.to_vec();
993            let mut minus = values.to_vec();
994            plus[variable] = if nav_state {
995                retract_nav_state(&values[variable], &increment).unwrap()
996            } else {
997                &values[variable] + &increment
998            };
999            minus[variable] = if nav_state {
1000                retract_nav_state(&values[variable], &(-&increment)).unwrap()
1001            } else {
1002                &values[variable] - &increment
1003            };
1004            let plus_refs = plus.iter().collect::<Vec<_>>();
1005            let minus_refs = minus.iter().collect::<Vec<_>>();
1006            let derivative = (factor.evaluate(&plus_refs).unwrap().residual
1007                - factor.evaluate(&minus_refs).unwrap().residual)
1008                / (2.0 * epsilon);
1009            jacobian.set_column(column, &derivative);
1010        }
1011        jacobian
1012    }
1013
1014    #[test]
1015    fn imu_extrinsics_include_rotation_and_lever_arm_terms() {
1016        let extrinsics = ImuExtrinsics {
1017            rotation_body_from_sensor: so3_exp(&Vector3::new(0.1, -0.2, 0.3)),
1018            translation_body_from_sensor: Vector3::new(0.4, -0.2, 0.1),
1019        };
1020        let measurement = ImuMeasurement {
1021            acceleration: Vector3::new(0.3, -0.1, 9.7),
1022            angular_velocity: Vector3::new(0.2, -0.3, 0.4),
1023        };
1024        let angular_acceleration = Vector3::new(0.05, 0.02, -0.04);
1025        let transformed = extrinsics.transform(measurement, angular_acceleration);
1026        let rotation = extrinsics.rotation_body_from_sensor;
1027        let omega = rotation * measurement.angular_velocity;
1028        let alpha = rotation * angular_acceleration;
1029        let expected_acceleration = rotation * measurement.acceleration
1030            - skew(&omega) * skew(&omega) * extrinsics.translation_body_from_sensor
1031            + skew(&extrinsics.translation_body_from_sensor) * alpha;
1032        assert!((transformed.acceleration - expected_acceleration).norm() < 1.0e-12);
1033        assert!((transformed.angular_velocity - omega).norm() < 1.0e-12);
1034
1035        let identity = ImuExtrinsics::identity().transform(measurement, angular_acceleration);
1036        assert_eq!(identity, measurement);
1037    }
1038
1039    #[test]
1040    fn specialized_factor_jacobians_match_finite_differences() {
1041        let state_i = NavState {
1042            rotation: so3_exp(&Vector3::new(0.2, -0.1, 0.3)),
1043            position: Vector3::new(1.0, -2.0, 0.5),
1044            velocity: Vector3::new(0.4, 0.1, -0.2),
1045        };
1046        let state_j = NavState {
1047            rotation: so3_exp(&Vector3::new(-0.1, 0.3, 0.2)),
1048            position: Vector3::new(1.3, -1.8, 0.7),
1049            velocity: Vector3::new(0.2, 0.5, -0.1),
1050        };
1051        let measurement = NavState {
1052            rotation: so3_exp(&Vector3::new(0.25, -0.05, 0.28)),
1053            position: Vector3::new(0.9, -1.9, 0.55),
1054            velocity: Vector3::new(0.35, 0.2, -0.15),
1055        };
1056        let value_i = encode_nav_state(&state_i);
1057        let value_j = encode_nav_state(&state_j);
1058
1059        let prior = NavStatePriorFactor::new(VariableId(0), measurement, Matrix9::identity());
1060        let prior_values = vec![value_i.clone()];
1061        let prior_refs = prior_values.iter().collect::<Vec<_>>();
1062        let prior_evaluation = prior.evaluate(&prior_refs).unwrap();
1063        let prior_numerical = numerical_factor_jacobian(&prior, &prior_values, 0, true);
1064        assert!(
1065            (&prior_evaluation.jacobians[0] - prior_numerical).norm() < 1.0e-5,
1066            "navigation prior analytic Jacobian disagrees with finite differences"
1067        );
1068
1069        let between_measurement = NavStateDelta {
1070            rotation: so3_exp(&Vector3::new(-0.2, 0.1, 0.15)),
1071            position: Vector3::new(0.2, 0.1, -0.05),
1072            velocity: Vector3::new(-0.1, 0.25, 0.08),
1073        };
1074        let between = NavStateBetweenFactor::new(
1075            VariableId(0),
1076            VariableId(1),
1077            between_measurement,
1078            Matrix9::identity(),
1079        );
1080        let between_values = vec![value_i.clone(), value_j.clone()];
1081        let between_refs = between_values.iter().collect::<Vec<_>>();
1082        let between_evaluation = between.evaluate(&between_refs).unwrap();
1083        for variable in 0..2 {
1084            let numerical = numerical_factor_jacobian(&between, &between_values, variable, true);
1085            assert!(
1086                (&between_evaluation.jacobians[variable] - numerical).norm() < 1.0e-5,
1087                "navigation between analytic Jacobian {variable} disagrees with finite differences"
1088            );
1089        }
1090
1091        let position_velocity =
1092            PositionVelocityFactor::new(VariableId(0), VariableId(1), 0.2, Matrix3::identity())
1093                .unwrap();
1094        let pv_evaluation = position_velocity.evaluate(&between_refs).unwrap();
1095        for variable in 0..2 {
1096            let numerical =
1097                numerical_factor_jacobian(&position_velocity, &between_values, variable, true);
1098            assert!(
1099                (&pv_evaluation.jacobians[variable] - numerical).norm() < 1.0e-7,
1100                "position/velocity analytic Jacobian {variable} disagrees with finite differences"
1101            );
1102        }
1103    }
1104
1105    #[test]
1106    fn bias_factors_match_mathematical_robotics_residuals() {
1107        let bias_i = ImuBias {
1108            accelerometer: Vector3::new(0.1, -0.2, 0.3),
1109            gyroscope: Vector3::new(-0.01, 0.02, 0.03),
1110        };
1111        let bias_j = ImuBias {
1112            accelerometer: Vector3::new(0.08, -0.18, 0.25),
1113            gyroscope: Vector3::new(-0.02, 0.01, 0.04),
1114        };
1115        let prior = BiasPriorFactor::new(VariableId(0), bias_j, Matrix6::identity());
1116        let prior_value = bias_i.vector();
1117        let prior_evaluation = prior.evaluate(&[&prior_value]).unwrap();
1118        assert!((prior_evaluation.residual - (bias_i.vector() - bias_j.vector())).norm() < 1.0e-12);
1119
1120        let between = BiasBetweenFactor::new(VariableId(0), VariableId(1), Matrix6::identity());
1121        let value_i = bias_i.vector();
1122        let value_j = bias_j.vector();
1123        let evaluation = between.evaluate(&[&value_i, &value_j]).unwrap();
1124        assert!((evaluation.residual - (bias_i.vector() - bias_j.vector())).norm() < 1.0e-12);
1125        let values = vec![value_i, value_j];
1126        for variable in 0..2 {
1127            let numerical = numerical_factor_jacobian(&between, &values, variable, false);
1128            assert!(
1129                (&evaluation.jacobians[variable] - numerical).norm() < 1.0e-9,
1130                "bias-between analytic Jacobian {variable} disagrees with finite differences"
1131            );
1132        }
1133    }
1134
1135    #[test]
1136    fn stationary_imu_prediction_cancels_gravity() {
1137        let mut measurement =
1138            PreintegratedImuMeasurement::new(ImuBias::zero(), ImuNoise::default());
1139        for _ in 0..100 {
1140            measurement
1141                .integrate(Vector3::new(0.0, 0.0, 9.81), Vector3::zeros(), 0.01)
1142                .unwrap();
1143        }
1144        let predicted = measurement.predict(
1145            &NavState::identity(),
1146            &ImuBias::zero(),
1147            Vector3::new(0.0, 0.0, -9.81),
1148        );
1149        assert!(predicted.position.norm() < 1.0e-9);
1150        assert!(predicted.velocity.norm() < 1.0e-9);
1151        assert!(measurement.covariance.trace() > 0.0);
1152    }
1153
1154    #[test]
1155    fn imu_factor_has_zero_residual_for_prediction() {
1156        let mut measurement =
1157            PreintegratedImuMeasurement::new(ImuBias::zero(), ImuNoise::default());
1158        measurement
1159            .integrate(
1160                Vector3::new(0.2, -0.1, 9.81),
1161                Vector3::new(0.01, 0.02, -0.01),
1162                0.1,
1163            )
1164            .unwrap();
1165        let state_i = NavState {
1166            rotation: so3_exp(&Vector3::new(0.2, -0.1, 0.3)),
1167            position: Vector3::new(1.0, -2.0, 0.5),
1168            velocity: Vector3::new(0.4, 0.1, -0.2),
1169        };
1170        let gravity = Vector3::new(0.0, 0.0, -9.81);
1171        let state_j = measurement.predict(&state_i, &ImuBias::zero(), gravity);
1172        let mut problem = Problem::new();
1173        let id_i = problem.add_variable(nav_state_variable(&state_i));
1174        let id_j = problem.add_variable(nav_state_variable(&state_j));
1175        let bias_id = problem.add_variable(imu_bias_variable(&ImuBias::zero()));
1176        let factor = ImuFactor::new(id_i, id_j, bias_id, measurement, gravity);
1177        let values = [
1178            problem.variable(id_i).unwrap().value(),
1179            problem.variable(id_j).unwrap().value(),
1180            problem.variable(bias_id).unwrap().value(),
1181        ];
1182        let evaluation = factor.evaluate(&values).unwrap();
1183        assert!(evaluation.residual.norm() < 1.0e-9);
1184        assert_eq!(evaluation.jacobians[0].shape(), (9, 9));
1185        assert_eq!(evaluation.jacobians[2].shape(), (9, 6));
1186        for variable in 0..3 {
1187            let numerical = factor.numerical_jacobian(&values, variable);
1188            assert!(
1189                (&evaluation.jacobians[variable] - numerical).norm() < 1.0e-5,
1190                "analytic IMU Jacobian {variable} disagrees with finite differences"
1191            );
1192        }
1193    }
1194
1195    #[test]
1196    fn trajectory_optimizer_preserves_consistent_prediction() {
1197        let bias = ImuBias {
1198            accelerometer: Vector3::new(0.01, -0.02, 0.03),
1199            gyroscope: Vector3::new(0.001, -0.002, 0.003),
1200        };
1201        let gravity = Vector3::new(0.0, 0.0, -9.81);
1202        let initial = NavState::identity();
1203        let mut measurement = PreintegratedImuMeasurement::new(bias, ImuNoise::default());
1204        measurement
1205            .integrate(
1206                Vector3::new(0.1, -0.2, 9.9),
1207                Vector3::new(0.02, 0.01, -0.03),
1208                0.1,
1209            )
1210            .unwrap();
1211        let predicted = measurement.predict(&initial, &bias, gravity);
1212        let result = optimize_imu_trajectory(
1213            &[initial.clone(), predicted.clone()],
1214            bias,
1215            &[measurement],
1216            gravity,
1217            &ImuTrajectoryConfig::default(),
1218        )
1219        .unwrap();
1220        assert!(nav_state_local(&result.states[0], &initial).norm() < 1.0e-9);
1221        assert!(nav_state_local(&result.states[1], &predicted).norm() < 1.0e-9);
1222        assert!((result.biases[0].vector() - bias.vector()).norm() < 1.0e-9);
1223        assert!((result.biases[1].vector() - bias.vector()).norm() < 1.0e-9);
1224    }
1225
1226    #[test]
1227    fn visual_state_priors_make_constant_accelerometer_bias_observable() {
1228        let gravity = Vector3::new(0.0, 0.0, -9.81);
1229        let true_bias = ImuBias {
1230            accelerometer: Vector3::new(0.2, -0.1, 0.05),
1231            gyroscope: Vector3::zeros(),
1232        };
1233        let linearization_bias = ImuBias::zero();
1234        let initial = NavState::identity();
1235        let mut states = vec![initial.clone()];
1236        let mut measurements = Vec::new();
1237        for _ in 0..4 {
1238            let mut measurement =
1239                PreintegratedImuMeasurement::new(linearization_bias, ImuNoise::default());
1240            for _ in 0..10 {
1241                measurement
1242                    .integrate(
1243                        Vector3::new(0.0, 0.0, 9.81) + true_bias.accelerometer,
1244                        Vector3::zeros(),
1245                        0.01,
1246                    )
1247                    .unwrap();
1248            }
1249            states.push(measurement.predict(states.last().unwrap(), &true_bias, gravity));
1250            measurements.push(measurement);
1251        }
1252        let mut visual_information = Matrix9::zeros();
1253        visual_information
1254            .fixed_view_mut::<6, 6>(0, 0)
1255            .copy_from(&(SMatrix::<f64, 6, 6>::identity() * 1.0e8));
1256        let result = optimize_imu_trajectory(
1257            &states,
1258            linearization_bias,
1259            &measurements,
1260            gravity,
1261            &ImuTrajectoryConfig {
1262                nav_measurement_information: Some(visual_information),
1263                bias_prior_information: Matrix6::identity() * 1.0e-6,
1264                bias_between_information: Matrix6::identity() * 1.0e6,
1265                ..ImuTrajectoryConfig::default()
1266            },
1267        )
1268        .unwrap();
1269        assert!(
1270            (result.biases[0].accelerometer - true_bias.accelerometer).norm() < 1.0e-4,
1271            "estimated bias {:?}",
1272            result.biases[0].accelerometer
1273        );
1274    }
1275}