嵌入式目标的卡尔曼滤波（在 Rust 中）

这是我的 kalman-clib 库的 Rust 版本，该库是一个针对微控制器的卡尔曼滤波实现，以及 libfixkalman C 库，用于 Q16.16 固定点卡尔曼滤波。它可选地使用 micromath 在 no_std 上进行平方根计算，并且如果需要可以使用 libm。根据配置，此 crate 可能需要 f32 / FPU 支持。

此实现使用静态分配的缓冲区进行所有矩阵运算。由于 Rust 中数组分配缺乏 const 泛型，此 crate 还提供了辅助宏来创建所需的数组。

no_std 与 std，alloc

此 crate 默认构建为 no_std。要构建具有 std 支持的版本，请运行

cargo build --features=std

独立于 std，您可以启用 alloc 功能。这启用了具有堆分配缓冲区的简化构建器

cargo build --features=alloc

示例

具有分配的目标（std 或 alloc）

当直接或通过 std 间接启用 alloc crate 功能时，一些构建器被启用，允许简单地创建过滤器。这应该有助于非嵌入式用例，或任何不需要显式管理缓冲区位置的用例，以便更容易开始

const NUM_STATES: usize = 3;
const NUM_CONTROLS: usize = 2;
const NUM_OBSERVATIONS: usize = 1;

fn example() {
    let builder = regular::builder::KalmanFilterBuilder::<NUM_STATES, f32>::default();
    let mut filter = builder.build();
    let mut control = builder.controls().build::<NUM_CONTROLS>();
    let mut measurement = builder.observations().build::<NUM_OBSERVATIONS>();

    // Set up the system dynamics, control matrices, observation matrices, ...

    // Filter!
    loop {
        // Update your control vector(s).
        control.control_vector_mut().apply(|u| {
            u[0] = 0.0;
            u[1] = 1.0;
        });

        // Update your measurement vectors.
        measurement.measurement_vector_mut().apply(|z| {
            z[0] = 42.0;
        });

        // Update prediction (without controls).
        filter.predict();

        // Apply any controls to the prediction.
        filter.control(&mut control);

        // Apply any measurements.
        filter.correct(&mut measurement);

        // Access the state
        let state = filter.state_vector();
        let covariance = filter.system_covariance();
    }
}

扩展卡尔曼滤波器

总体设置保持不变，但是 predict 和 correct 方法被它们的非线性对应项所替换

const NUM_STATES: usize = 3;
const NUM_OBSERVATIONS: usize = 1;

fn example() {
    let builder = extended::builder::KalmanFilterBuilder::<NUM_STATES, f32>::default();
    let mut filter = builder.build();
    let mut measurement = builder.observations().build::<NUM_OBSERVATIONS>();

    // The time step of our simulation.
    const DELTA_T: f32 = 0.1;

    // Set up the initial state vector.
    filter.state_vector_mut().apply(|vec| {
        vec.set_row(0, 0.0);
        vec.set_row(1, 0.0);
        vec.set_row(2, 1.0);
        vec.set_row(3, 1.0);
    });

    // Set up the initial estimate covariance as an identity matrix.
    filter.estimate_covariance_mut().make_identity();

    // Set up the process noise covariance matrix as an identity matrix.
    measurement
        .measurement_noise_covariance_mut()
        .make_scalar(1.0);

    // Set up the measurement noise covariance.
    measurement.measurement_noise_covariance_mut().apply(|mat| {
        mat.set_value(1.0); // matrix is 1x1
    });

    // Simulate
    for step in 1..=100 {
        let time = step as f32 * DELTA_T;

        // Update the system transition Jacobian matrix.
        filter.state_transition_mut().apply(|mat| {
            mat.make_identity();
            mat.set(0, 2, DELTA_T);
            mat.set(1, 3, DELTA_T);
        });

        // Perform a nonlinear prediction step.
        filter.predict_nonlinear(|state, next| {
            // Simple constant velocity model.
            next[0] = state[0] + state[2] * DELTA_T;
            next[1] = state[1] + state[3] * DELTA_T;
            next[2] = state[2];
            next[3] = state[3];
        });

        // Prepare a measurement.
        measurement.measurement_vector_mut().apply(|vec| {
            // Noise setup.
            let mut rng = rand::thread_rng();
            let measurement_noise = Normal::new(0.0, 0.5).unwrap();

            // Perform a noisy measurement of the (simulated) position.
            let z = (time.powi(2) + time.powi(2)).sqrt();
            let noise = measurement_noise.sample(&mut rng);

            vec.set_value(z + noise);
        });

        // Update the observation Jacobian.
        measurement.observation_matrix_mut().apply(|mat| {
            let x = filter.state_vector().get_row(0);
            let y = filter.state_vector().get_row(1);

            let norm = (x.powi(2) + y.powi(2)).sqrt();
            let dx = x / norm;
            let dy = y / norm;

            mat.set_col(0, dx);
            mat.set_col(1, dy);
            mat.set_col(2, 0.0);
            mat.set_col(3, 0.0);
        });

        // Apply nonlinear correction step.
        filter.correct_nonlinear(&mut measurement, |state, observation| {
            // Transform the state into an observation.
            let x = state.get_row(0);
            let y = state.get_row(1);
            let z = (x.powi(2) + y.powi(2)).sqrt();
            observation.set_value(z);
        });
    }
}

要查看一个更真实的EKF示例，该示例模拟了移动对象的雷达测量，请参阅radar-2d示例。

cargo run --example radar-2d --features=std

嵌入式目标

STM32F303微控制器的示例可以在xbuild-tests/stm32目录中找到。它展示了定点和浮点支持。

Q16.16定点

使用fixedcrate功能运行fixed示例。这启用了I16F16类型支持，类似于libfixkalman C库。

cargo run --example fixed --features=fixed

重力常数估算示例

要运行gravity模拟示例，运行以下命令：

cargo run --example gravity --features=std
cargo run --example gravity --features=std,libm

这将通过观察自由落体物体的位置来估计（地球的）重力常数（g ≈ 9.807 m/s²）。执行时，应打印类似以下内容：

At t = 0, predicted state: s = 3 m, v = 6 m/s, a = 6 m/s²
At t = 0, measurement: s = 0 m, noise ε = 0.13442 m
At t = 0, corrected state: s = 0.908901 m, v = 3.6765568 m/s, a = 5.225519 m/s²
At t = 1, predicted state: s = 7.1982174 m, v = 8.902076 m/s, a = 5.225519 m/s²
At t = 1, measurement: s = 4.905 m, noise ε = 0.45847 m
At t = 1, corrected state: s = 5.6328573 m, v = 7.47505 m/s, a = 4.5993752 m/s²
At t = 2, predicted state: s = 15.407595 m, v = 12.074425 m/s, a = 4.5993752 m/s²
At t = 2, measurement: s = 19.62 m, noise ε = -0.56471 m
At t = 2, corrected state: s = 18.50683 m, v = 14.712257 m/s, a = 5.652767 m/s²
At t = 3, predicted state: s = 36.04547 m, v = 20.365025 m/s, a = 5.652767 m/s²
At t = 3, measurement: s = 44.145 m, noise ε = 0.21554 m
At t = 3, corrected state: s = 42.8691 m, v = 25.476515 m/s, a = 7.3506646 m/s²
At t = 4, predicted state: s = 72.02094 m, v = 32.82718 m/s, a = 7.3506646 m/s²
At t = 4, measurement: s = 78.48 m, noise ε = 0.079691 m
At t = 4, corrected state: s = 77.09399 m, v = 36.10087 m/s, a = 8.258889 m/s²
At t = 5, predicted state: s = 117.3243 m, v = 44.359756 m/s, a = 8.258889 m/s²
At t = 5, measurement: s = 122.63 m, noise ε = -0.32692 m
At t = 5, corrected state: s = 120.94025 m, v = 46.38022 m/s, a = 8.736543 m/s²
At t = 6, predicted state: s = 171.68874 m, v = 55.11676 m/s, a = 8.736543 m/s²
At t = 6, measurement: s = 176.58 m, noise ε = -0.1084 m
At t = 6, corrected state: s = 174.93135 m, v = 56.704926 m/s, a = 9.062785 m/s²
At t = 7, predicted state: s = 236.16766 m, v = 65.76771 m/s, a = 9.062785 m/s²
At t = 7, measurement: s = 240.35 m, noise ε = 0.085656 m
At t = 7, corrected state: s = 238.87048 m, v = 66.942894 m/s, a = 9.276019 m/s²
At t = 8, predicted state: s = 310.4514 m, v = 76.21891 m/s, a = 9.276019 m/s²
At t = 8, measurement: s = 313.92 m, noise ε = 0.8946 m
At t = 8, corrected state: s = 313.03793 m, v = 77.22877 m/s, a = 9.44006 m/s²
At t = 9, predicted state: s = 394.98672 m, v = 86.66882 m/s, a = 9.44006 m/s²
At t = 9, measurement: s = 397.31 m, noise ε = 0.69236 m
At t = 9, corrected state: s = 396.6648 m, v = 87.26297 m/s, a = 9.527418 m/s²
At t = 10, predicted state: s = 488.69147 m, v = 96.79039 m/s, a = 9.527418 m/s²
At t = 10, measurement: s = 490.5 m, noise ε = -0.33747 m
At t = 10, corrected state: s = 489.46213 m, v = 97.03994 m/s, a = 9.560934 m/s²
At t = 11, predicted state: s = 591.28253 m, v = 106.600876 m/s, a = 9.560934 m/s²
At t = 11, measurement: s = 593.51 m, noise ε = 0.75873 m
At t = 11, corrected state: s = 592.75964 m, v = 107.04147 m/s, a = 9.615404 m/s²
At t = 12, predicted state: s = 704.6088 m, v = 116.656876 m/s, a = 9.615404 m/s²
At t = 12, measurement: s = 706.32 m, noise ε = 0.18135 m
At t = 12, corrected state: s = 705.4952 m, v = 116.90193 m/s, a = 9.643473 m/s²
At t = 13, predicted state: s = 827.2188 m, v = 126.5454 m/s, a = 9.643473 m/s²
At t = 13, measurement: s = 828.94 m, noise ε = -0.015764 m
At t = 13, corrected state: s = 827.97705 m, v = 126.74077 m/s, a = 9.66432 m/s²
At t = 14, predicted state: s = 959.55 m, v = 136.40509 m/s, a = 9.66432 m/s²
At t = 14, measurement: s = 961.38 m, noise ε = 0.17869 m
At t = 14, corrected state: s = 960.39984 m, v = 136.6101 m/s, a = 9.684802 m/s²