multicalc 0.10.0

Math for real-time embedded systems, in stable no_std Rust: state estimation, control, kinematics, Lie groups, autodiff, and linear algebra — from 64-bit servers to bare-metal microcontrollers
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175

#![allow(clippy::unwrap_used, clippy::expect_used, clippy::panic)]

//! Discretization + matrix-exponential invariants and closed-form checks.

use multicalc::discretization::{q_discrete_white_noise, van_loan, zoh};
use multicalc::error::LinalgError;
use multicalc::linear_algebra::{Matrix, Matrix2D, Matrix3D, Matrix4D};
use multicalc::scalar::Dual;
use proptest::prelude::*;

fn matrix3(entries: [[f64; 3]; 3]) -> Matrix3D {
    Matrix::new(entries)
}

#[test]
fn expm_zero_is_identity() {
    let exponential = Matrix4D::<f64>::zeros().expm().unwrap();
    for row in 0..4 {
        for column in 0..4 {
            let expected = if row == column { 1.0 } else { 0.0 };
            assert!((exponential[(row, column)] - expected).abs() < 1e-12);
        }
    }
}

#[test]
fn expm_diagonal_is_elementwise_exp() {
    let diagonal = matrix3([[0.5, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, 2.0]]);
    let exponential = diagonal.expm().unwrap();
    for (index, entry) in [0.5_f64, -1.0, 2.0].into_iter().enumerate() {
        assert!((exponential[(index, index)] - entry.exp()).abs() < 1e-10);
    }
}

#[test]
fn expm_rejects_non_finite_input() {
    for value in [f64::NAN, f64::INFINITY, f64::NEG_INFINITY] {
        let matrix = Matrix::<1, 1>::new([[value]]);
        assert_eq!(matrix.expm().err(), Some(LinalgError::NonFinite));
    }
}

#[test]
fn expm_derivative_finite_and_correct() {
    // d/dx expm(x·M)|_{x=0} = M. One Dual through expm; compare to central FD.
    let generator = matrix3([[0.1, 0.4, -0.2], [0.0, -0.3, 0.5], [0.2, 0.1, 0.05]]);
    let autodiff =
        Matrix3D::<Dual<f64>>::from_fn(|row, column| Dual::new(0.0, generator[(row, column)]))
            .expm()
            .unwrap();
    for row in 0..3 {
        for column in 0..3 {
            let cell = autodiff[(row, column)];
            assert!(cell.deriv.is_finite());
            assert!((cell.deriv - generator[(row, column)]).abs() < 1e-9);
        }
    }
}

#[test]
fn zoh_double_integrator() {
    let state_matrix = Matrix2D::new([[0.0, 1.0], [0.0, 0.0]]);
    let input_matrix = Matrix::<2, 1>::new([[0.0], [1.0]]);
    let timestep = 0.1;
    let (discrete_state, discrete_input) =
        zoh::<2, 1, 3, f64>(state_matrix, input_matrix, timestep).unwrap();
    let expected_state = [[1.0, timestep], [0.0, 1.0]];
    for (row, entries) in expected_state.iter().enumerate() {
        for (column, &expected) in entries.iter().enumerate() {
            assert!((discrete_state[(row, column)] - expected).abs() < 1e-9);
        }
    }
    assert!((discrete_input[(0, 0)] - timestep * timestep / 2.0).abs() < 1e-9);
    assert!((discrete_input[(1, 0)] - timestep).abs() < 1e-9);
}

#[test]
fn van_loan_qd_symmetric_and_f_matches_expm() {
    let state_matrix = matrix3([[-0.5, 0.2, 0.0], [0.1, -0.3, 0.4], [0.0, 0.2, -0.6]]);
    let continuous_noise = matrix3([[0.2, 0.0, 0.0], [0.0, 0.3, 0.0], [0.0, 0.0, 0.1]]);
    let timestep = 0.05;
    let (discrete_state, discrete_noise) =
        van_loan::<3, 6, f64>(state_matrix, continuous_noise, timestep).unwrap();
    let expected_from_expm = state_matrix.scale(timestep).expm().unwrap();
    for row in 0..3 {
        for column in 0..3 {
            assert!(
                (discrete_state[(row, column)] - expected_from_expm[(row, column)]).abs() < 1e-9
            );
            assert!((discrete_noise[(row, column)] - discrete_noise[(column, row)]).abs() < 1e-10);
        }
    }
}

#[test]
fn zoh_rejects_negative_timestep() {
    let state_matrix = Matrix::<1, 1>::new([[1.0]]);
    let input_matrix = Matrix::<1, 1>::new([[1.0]]);

    assert_eq!(
        zoh::<1, 1, 2, f64>(state_matrix, input_matrix, -0.1).err(),
        Some(LinalgError::InvalidTimestep)
    );
}

#[test]
fn van_loan_rejects_negative_timestep() {
    let state_matrix = Matrix::<1, 1>::new([[1.0]]);
    let continuous_noise = Matrix::<1, 1>::new([[1.0]]);

    assert_eq!(
        van_loan::<1, 2, f64>(state_matrix, continuous_noise, -0.1).err(),
        Some(LinalgError::InvalidTimestep)
    );
}

#[test]
fn discretization_rejects_non_finite_timesteps() {
    let state_matrix = Matrix::<1, 1>::new([[1.0]]);
    let input_or_noise_matrix = Matrix::<1, 1>::new([[1.0]]);

    for timestep in [f64::NAN, f64::INFINITY, f64::NEG_INFINITY] {
        assert_eq!(
            zoh::<1, 1, 2, f64>(state_matrix, input_or_noise_matrix, timestep).err(),
            Some(LinalgError::InvalidTimestep)
        );
        assert_eq!(
            van_loan::<1, 2, f64>(state_matrix, input_or_noise_matrix, timestep).err(),
            Some(LinalgError::InvalidTimestep)
        );
    }
}

#[test]
fn zero_timestep_is_valid() {
    let state_matrix = Matrix::<1, 1>::new([[1.0]]);
    let input_or_noise_matrix = Matrix::<1, 1>::new([[1.0]]);

    let (zoh_state, zoh_input) =
        zoh::<1, 1, 2, f64>(state_matrix, input_or_noise_matrix, 0.0).unwrap();
    assert_eq!(zoh_state, Matrix::identity());
    assert_eq!(zoh_input, Matrix::zeros());

    let (van_loan_state, van_loan_noise) =
        van_loan::<1, 2, f64>(state_matrix, input_or_noise_matrix, 0.0).unwrap();
    assert_eq!(van_loan_state, Matrix::identity());
    assert_eq!(van_loan_noise, Matrix::zeros());
}

#[test]
fn qdwn_matches_closed_form() {
    let timestep = 0.1;
    let variance = 2.0;
    let noise = q_discrete_white_noise::<2, f64>(timestep, variance);
    assert!((noise[(0, 0)] - variance * timestep.powi(4) / 4.0).abs() < 1e-15);
    assert!((noise[(0, 1)] - variance * timestep.powi(3) / 2.0).abs() < 1e-15);
    assert!((noise[(1, 0)] - variance * timestep.powi(3) / 2.0).abs() < 1e-15);
    assert!((noise[(1, 1)] - variance * timestep * timestep).abs() < 1e-15);
}

proptest! {
    #[test]
    fn expm_times_neg_expm_is_identity(
        v in prop::collection::vec(-0.6f64..0.6, 9)
    ) {
        let matrix = Matrix3D::from_fn(|row, column| v[row * 3 + column]);
        let product = matrix.expm().unwrap() * matrix.scale(-1.0).expm().unwrap();
        for row in 0..3 {
            for column in 0..3 {
                let expected = if row == column { 1.0 } else { 0.0 };
                prop_assert!((product[(row, column)] - expected).abs() < 1e-9);
            }
        }
    }
}