tests.rs
raw
use super::*;
// Simple 1D position tracking: state = [position, velocity]
struct ConstantVelocity;
impl ekf::ProcessModel<2, 0> for ConstantVelocity {
fn predict_next(&self, state: &[f64; 2], _control: &[f64; 0], dt: f64) -> [f64; 2] {
[state[0] + state[1] * dt, state[1]]
}
fn process_noise_std(&self) -> [f64; 2] {
[0.1, 0.01]
}
}
struct PositionSensor;
impl ekf::MeasurementModel<2, 1> for PositionSensor {
fn expected_measurement(&self, state: &[f64; 2]) -> [f64; 1] {
[state[0]]
}
fn measurement_noise_std(&self) -> [f64; 1] {
[0.5]
}
}
#[test]
fn test_ekf_tracking() {
let mut ekf =
ekf::ExtendedKalmanFilter::new([0.0, 1.0], [1.0, 0.5], ConstantVelocity, PositionSensor);
// Simulate for a few steps
for i in 0..10 {
ekf.predict(&[], 0.1);
// Simulate noisy measurement of true position
let true_pos = (i as f64 + 1.0) * 0.1;
let measurement = [true_pos + 0.05];
ekf.update(&measurement).unwrap();
}
let state = ekf.estimate();
// Should be tracking roughly position=1.0, velocity=1.0
assert!((state[0] - 1.0).abs() < 0.3);
assert!((state[1] - 1.0).abs() < 0.3);
}
#[test]
fn test_numerical_jacobian() {
// Test that numerical differentiation works correctly
fn f(x: &[f64; 2]) -> [f64; 2] {
[x[0] * x[0], x[0] * x[1]]
}
let jac = ekf::numerical_jacobian(f, &[2.0, 3.0]);
// df1/dx0 = 2*x0 = 4
assert!((jac[0][0] - 4.0).abs() < 1e-6);
// df1/dx1 = 0
assert!(jac[1][0].abs() < 1e-6);
// df2/dx0 = x1 = 3
assert!((jac[0][1] - 3.0).abs() < 1e-6);
// df2/dx1 = x0 = 2
assert!((jac[1][1] - 2.0).abs() < 1e-6);
}
// Particle filter tests
struct SimpleMotion;
impl particle::ProcessModel<[f64; 2], [f64; 0]> for SimpleMotion {
fn sample_next<R: rand::Rng>(
&self,
state: &[f64; 2],
_control: &[f64; 0],
dt: f64,
rng: &mut R,
) -> [f64; 2] {
use rand_distr::{Distribution, StandardNormal};
let noise: f64 = StandardNormal.sample(rng);
[state[0] + state[1] * dt + noise * 0.1 * dt.sqrt(), state[1]]
}
}
struct SimpleSensor;
impl particle::MeasurementModel<[f64; 2], [f64; 1]> for SimpleSensor {
fn likelihood(&self, state: &[f64; 2], measurement: &[f64; 1]) -> f64 {
let error = state[0] - measurement[0];
(-0.5 * (error / 0.5).powi(2)).exp()
}
}
struct UniformInitial {
center: [f64; 2],
spread: f64,
}
impl rand::distr::Distribution<[f64; 2]> for UniformInitial {
fn sample<R: rand::Rng + ?Sized>(&self, rng: &mut R) -> [f64; 2] {
[
self.center[0] + (rng.random::<f64>() - 0.5) * self.spread,
self.center[1] + (rng.random::<f64>() - 0.5) * self.spread * 0.2,
]
}
}
#[test]
fn test_particle_filter() {
use rand::SeedableRng;
use rand_chacha::ChaCha8Rng;
let initial = UniformInitial {
center: [0.0, 1.0],
spread: 2.0,
};
let mut pf = particle::ParticleFilter::new(
initial,
particle::Config {
num_particles: 500,
resample_threshold: 0.5,
},
SimpleMotion,
SimpleSensor,
ChaCha8Rng::seed_from_u64(42),
);
// Run a few steps
for i in 0..10 {
pf.predict(&[], 0.1);
let measurement = [(i as f64 + 1.0) * 0.1];
pf.update(&measurement).unwrap();
}
let state = pf.estimate();
// Should be roughly tracking the measurements
assert!((state[0] - 1.0).abs() < 0.5);
}
#[test]
fn test_uncertainty_decreases() {
let mut ekf = ekf::ExtendedKalmanFilter::new(
[0.0, 1.0],
[10.0, 5.0], // Start very uncertain
ConstantVelocity,
PositionSensor,
);
let initial_uncertainty = ekf.uncertainty();
// Give it some measurements
for _ in 0..20 {
ekf.predict(&[], 0.1);
ekf.update(&[0.1]).unwrap();
}
let final_uncertainty = ekf.uncertainty();
// Position uncertainty should decrease
assert!(final_uncertainty[0] < initial_uncertainty[0]);
}
#[test]
fn test_ekf_singular_covariance_error() {
// Test that we get a proper error with zero measurement noise AND zero state covariance
// S = H * P * H^T + R, so both P and R must be zero for S to be singular
struct ZeroNoiseSensor;
impl ekf::MeasurementModel<2, 1> for ZeroNoiseSensor {
fn expected_measurement(&self, state: &[f64; 2]) -> [f64; 1] {
[state[0]]
}
fn measurement_noise_std(&self) -> [f64; 1] {
[0.0] // Zero measurement noise
}
}
let mut ekf = ekf::ExtendedKalmanFilter::new(
[0.0, 1.0],
[0.0, 0.0], // Zero initial uncertainty makes P = 0
ConstantVelocity,
ZeroNoiseSensor,
);
// Skip predict to keep P at zero (predict adds process noise)
let result = ekf.update(&[0.1]);
assert!(matches!(
result,
Err(EstimatorError::SingularInnovationCovariance)
));
}
#[test]
fn test_particle_degeneracy_error() {
use rand::SeedableRng;
use rand_chacha::ChaCha8Rng;
// A sensor that always returns 0 likelihood
struct ImpossibleSensor;
impl particle::MeasurementModel<[f64; 2], [f64; 1]> for ImpossibleSensor {
fn likelihood(&self, _state: &[f64; 2], _measurement: &[f64; 1]) -> f64 {
0.0 // Nothing ever matches
}
}
let initial = UniformInitial {
center: [0.0, 1.0],
spread: 2.0,
};
let mut pf = particle::ParticleFilter::new(
initial,
particle::Config {
num_particles: 100,
resample_threshold: 0.5,
},
SimpleMotion,
ImpossibleSensor,
ChaCha8Rng::seed_from_u64(42),
);
let result = pf.update(&[0.0]);
assert!(matches!(result, Err(EstimatorError::ParticleDegeneracy)));
}
// ========================================================================
// Comprehensive Tests for Correctness, Accuracy, and Resilience
// ========================================================================
mod linear_system_tests {
//! For linear systems, the EKF reduces to the standard Kalman Filter
//! and should be optimal. These tests verify correct behavior.
use super::*;
use rand::{Rng, SeedableRng};
use rand_chacha::ChaCha8Rng;
/// Linear constant-velocity model with configurable noise
struct LinearMotion {
process_noise: [f64; 2],
}
impl ekf::ProcessModel<2, 0> for LinearMotion {
fn predict_next(&self, state: &[f64; 2], _: &[f64; 0], dt: f64) -> [f64; 2] {
[state[0] + state[1] * dt, state[1]]
}
fn process_noise_std(&self) -> [f64; 2] {
self.process_noise
}
// Analytical Jacobian for linear system (should match numerical)
fn prediction_sensitivity(&self, _: &[f64; 2], _: &[f64; 0], dt: f64) -> [[f64; 2]; 2] {
[[1.0, 0.0], [dt, 1.0]]
}
}
struct LinearSensor {
noise_std: f64,
}
impl ekf::MeasurementModel<2, 1> for LinearSensor {
fn expected_measurement(&self, state: &[f64; 2]) -> [f64; 1] {
[state[0]]
}
fn measurement_noise_std(&self) -> [f64; 1] {
[self.noise_std]
}
fn measurement_sensitivity(&self, _: &[f64; 2]) -> [[f64; 1]; 2] {
[[1.0], [0.0]]
}
}
#[test]
fn test_ekf_tracks_constant_velocity_exactly() {
// With no noise and perfect initial conditions, EKF should track perfectly
let mut ekf = ekf::ExtendedKalmanFilter::new(
[0.0, 1.0], // Start at origin, velocity = 1
[0.01, 0.01],
LinearMotion {
process_noise: [0.001, 0.001],
},
LinearSensor { noise_std: 0.01 },
);
let dt = 0.1;
for i in 1..=100 {
ekf.predict(&[], dt);
// Perfect measurement of true position
let true_pos = i as f64 * dt;
ekf.update(&[true_pos]).unwrap();
let est = ekf.estimate();
assert!(
(est[0] - true_pos).abs() < 0.1,
"Position error too large at step {}: {} vs {}",
i,
est[0],
true_pos
);
assert!(
(est[1] - 1.0).abs() < 0.1,
"Velocity error too large at step {}: {}",
i,
est[1]
);
}
}
#[test]
fn test_ekf_converges_from_wrong_initial_estimate() {
// Start with completely wrong estimate, should converge
let mut ekf = ekf::ExtendedKalmanFilter::new(
[100.0, -5.0], // Way off!
[50.0, 10.0], // High uncertainty
LinearMotion {
process_noise: [0.1, 0.01],
},
LinearSensor { noise_std: 0.5 },
);
let dt = 0.1;
let true_velocity = 1.0;
for i in 1..=200 {
ekf.predict(&[], dt);
let true_pos = i as f64 * dt * true_velocity;
// Add some noise to measurements
let noise = (i as f64 * 0.1).sin() * 0.3;
ekf.update(&[true_pos + noise]).unwrap();
}
let est = ekf.estimate();
let true_final_pos = 200.0 * dt * true_velocity;
// Should have converged to within a few sigma
assert!(
(est[0] - true_final_pos).abs() < 2.0,
"Failed to converge: position {} vs {}",
est[0],
true_final_pos
);
assert!(
(est[1] - true_velocity).abs() < 0.5,
"Failed to converge: velocity {} vs {}",
est[1],
true_velocity
);
}
#[test]
fn test_ekf_uncertainty_is_consistent() {
// Run Monte Carlo simulations and verify errors match uncertainty
let mut rng = ChaCha8Rng::seed_from_u64(12345);
let num_trials = 100;
let mut position_errors: Vec<f64> = Vec::new();
let mut normalized_errors: Vec<f64> = Vec::new();
for _ in 0..num_trials {
let mut ekf = ekf::ExtendedKalmanFilter::new(
[0.0, 1.0],
[1.0, 0.5],
LinearMotion {
process_noise: [0.1, 0.01],
},
LinearSensor { noise_std: 0.5 },
);
let dt = 0.1;
let true_velocity = 1.0;
for i in 1..=50 {
ekf.predict(&[], dt);
let true_pos = i as f64 * dt * true_velocity;
// Gaussian-ish noise using multiple uniform samples
let noise: f64 = (0..12).map(|_| rng.random::<f64>() - 0.5).sum::<f64>() * 0.5;
ekf.update(&[true_pos + noise]).unwrap();
}
let est = ekf.estimate();
let unc = ekf.uncertainty();
let true_final_pos = 50.0 * dt * true_velocity;
let error = est[0] - true_final_pos;
position_errors.push(error);
normalized_errors.push(error / unc[0]);
}
// Check that ~68% of errors are within 1 sigma (allowing some slack)
let within_1_sigma = normalized_errors.iter().filter(|e| e.abs() < 1.0).count();
let fraction = within_1_sigma as f64 / num_trials as f64;
assert!(
fraction > 0.5 && fraction < 0.9,
"Uncertainty inconsistent: {}% within 1 sigma (expected ~68%)",
fraction * 100.0
);
// Check RMS error is reasonable
let rms_error =
(position_errors.iter().map(|e| e * e).sum::<f64>() / num_trials as f64).sqrt();
assert!(rms_error < 1.0, "RMS error too large: {}", rms_error);
}
#[test]
fn test_pf_converges_from_wrong_initial_estimate() {
use rand::SeedableRng;
use rand_chacha::ChaCha8Rng;
struct PFMotion;
impl particle::ProcessModel<[f64; 2], [f64; 0]> for PFMotion {
fn sample_next<R: rand::Rng>(
&self,
state: &[f64; 2],
_: &[f64; 0],
dt: f64,
rng: &mut R,
) -> [f64; 2] {
use rand_distr::{Distribution, StandardNormal};
let n1: f64 = StandardNormal.sample(rng);
let n2: f64 = StandardNormal.sample(rng);
// Higher process noise to allow particles to migrate
let noise_pos = n1 * 0.5 * dt.sqrt();
let noise_vel = n2 * 0.1 * dt.sqrt();
[state[0] + state[1] * dt + noise_pos, state[1] + noise_vel]
}
}
struct PFSensor;
impl particle::MeasurementModel<[f64; 2], [f64; 1]> for PFSensor {
fn likelihood(&self, state: &[f64; 2], measurement: &[f64; 1]) -> f64 {
let error = state[0] - measurement[0];
// Wider likelihood allows recovery from bad initial guess
(-0.5 * (error / 2.0).powi(2)).exp()
}
}
// Start particles centered at wrong location (but not impossibly far)
struct WrongInitial;
impl rand::distr::Distribution<[f64; 2]> for WrongInitial {
fn sample<R: rand::Rng + ?Sized>(&self, rng: &mut R) -> [f64; 2] {
let r1: f64 = rng.random();
let r2: f64 = rng.random();
[
10.0 + (r1 - 0.5) * 5.0, // Centered at 10, spread 5
-1.0 + (r2 - 0.5) * 1.0, // Wrong velocity
]
}
}
let mut pf = particle::ParticleFilter::new(
WrongInitial,
particle::Config {
num_particles: 2000,
resample_threshold: 0.5,
},
PFMotion,
PFSensor,
ChaCha8Rng::seed_from_u64(99),
);
let dt = 0.1;
for i in 1..=300 {
pf.predict(&[], dt);
let true_pos = i as f64 * dt;
pf.update(&[true_pos]).unwrap();
}
let est = pf.estimate();
let true_final = 300.0 * dt;
assert!(
(est[0] - true_final).abs() < 3.0,
"PF failed to converge: {} vs {}",
est[0],
true_final
);
assert!(
(est[1] - 1.0).abs() < 1.0,
"PF velocity estimate wrong: {} vs 1.0",
est[1]
);
}
}
mod nonlinear_system_tests {
//! Test on genuinely nonlinear systems where EKF approximation matters
use super::*;
use rand::SeedableRng;
use rand_chacha::ChaCha8Rng;
/// 2D robot with [x, y, heading] state and [velocity, turn_rate] control
struct UnicycleModel {
process_noise: [f64; 3],
}
impl ekf::ProcessModel<3, 2> for UnicycleModel {
fn predict_next(&self, state: &[f64; 3], control: &[f64; 2], dt: f64) -> [f64; 3] {
let [x, y, theta] = *state;
let [v, omega] = *control;
[
x + v * theta.cos() * dt,
y + v * theta.sin() * dt,
theta + omega * dt,
]
}
fn process_noise_std(&self) -> [f64; 3] {
self.process_noise
}
}
/// Range-bearing sensor measuring distance and angle to a landmark
struct RangeBearingSensor {
landmark: [f64; 2],
range_noise: f64,
bearing_noise: f64,
}
impl ekf::MeasurementModel<3, 2> for RangeBearingSensor {
fn expected_measurement(&self, state: &[f64; 3]) -> [f64; 2] {
let dx = self.landmark[0] - state[0];
let dy = self.landmark[1] - state[1];
let range = (dx * dx + dy * dy).sqrt();
let bearing = dy.atan2(dx) - state[2];
[range, bearing]
}
fn measurement_noise_std(&self) -> [f64; 2] {
[self.range_noise, self.bearing_noise]
}
}
#[test]
fn test_ekf_nonlinear_circular_motion() {
let mut ekf = ekf::ExtendedKalmanFilter::new(
[0.0, 0.0, 0.0],
[0.5, 0.5, 0.2],
UnicycleModel {
process_noise: [0.05, 0.05, 0.02],
},
RangeBearingSensor {
landmark: [5.0, 5.0],
range_noise: 0.3,
bearing_noise: 0.1,
},
);
let dt = 0.05;
let velocity = 1.0;
let turn_rate = 0.5; // Circular motion
let mut true_state: [f64; 3] = [0.0, 0.0, 0.0];
for _ in 0..200 {
// True motion
true_state[0] += velocity * true_state[2].cos() * dt;
true_state[1] += velocity * true_state[2].sin() * dt;
true_state[2] += turn_rate * dt;
// Filter step
ekf.predict(&[velocity, turn_rate], dt);
// True measurement
let dx = 5.0 - true_state[0];
let dy = 5.0 - true_state[1];
let range = (dx * dx + dy * dy).sqrt();
let bearing = dy.atan2(dx) - true_state[2];
ekf.update(&[range, bearing]).unwrap();
}
let est = ekf.estimate();
// Should track within reasonable bounds for nonlinear system
assert!(
(est[0] - true_state[0]).abs() < 1.0,
"X error: {} vs {}",
est[0],
true_state[0]
);
assert!(
(est[1] - true_state[1]).abs() < 1.0,
"Y error: {} vs {}",
est[1],
true_state[1]
);
}
#[test]
fn test_pf_nonlinear_handles_ambiguity() {
// Particle filter should handle multimodal distributions
// where EKF might fail
struct CircularMotion;
impl particle::ProcessModel<[f64; 2], ()> for CircularMotion {
fn sample_next<R: rand::Rng>(
&self,
state: &[f64; 2],
_: &(),
dt: f64,
rng: &mut R,
) -> [f64; 2] {
use rand_distr::{Distribution, StandardNormal};
let angle = state[1] + 0.1 * dt;
let n: f64 = StandardNormal.sample(rng);
[state[0] + n * 0.05, angle]
}
}
// Sensor that only measures distance from origin (ambiguous!)
struct DistanceOnlySensor;
impl particle::MeasurementModel<[f64; 2], f64> for DistanceOnlySensor {
fn likelihood(&self, state: &[f64; 2], measurement: &f64) -> f64 {
// state[0] is radius, state[1] is angle
let error = state[0] - measurement;
(-0.5 * (error / 0.2).powi(2)).exp()
}
}
struct RadialInitial;
impl rand::distr::Distribution<[f64; 2]> for RadialInitial {
fn sample<R: rand::Rng + ?Sized>(&self, rng: &mut R) -> [f64; 2] {
let r1: f64 = rng.random();
let r2: f64 = rng.random();
[
5.0 + (r1 - 0.5) * 2.0, // radius ~5
r2 * std::f64::consts::TAU, // any angle
]
}
}
let mut pf = particle::ParticleFilter::new(
RadialInitial,
particle::Config {
num_particles: 500,
resample_threshold: 0.5,
},
CircularMotion,
DistanceOnlySensor,
ChaCha8Rng::seed_from_u64(777),
);
// Measure distance = 5 repeatedly
for _ in 0..50 {
pf.predict(&(), 0.1);
pf.update(&5.0).unwrap();
}
let est = pf.estimate();
// Radius should converge to 5, angle could be anything
assert!(
(est[0] - 5.0).abs() < 0.5,
"Radius estimate wrong: {}",
est[0]
);
}
}
mod resilience_tests {
//! Test filter behavior under adverse conditions
use super::*;
#[test]
fn test_ekf_handles_outliers() {
// Note: Standard EKF has no built-in outlier rejection.
// This test verifies the filter can RECOVER from outliers,
// not that it ignores them.
let mut ekf = ekf::ExtendedKalmanFilter::new(
[0.0, 1.0],
[1.0, 0.5],
ConstantVelocity,
PositionSensor,
);
let dt = 0.1;
// Normal tracking
for i in 1..=20 {
ekf.predict(&[], dt);
ekf.update(&[i as f64 * dt]).unwrap();
}
let before_outlier = ekf.estimate();
// Inject massive outlier
ekf.predict(&[], dt);
ekf.update(&[1000.0]).unwrap();
let after_outlier = ekf.estimate();
// Filter WILL be affected by outlier (this is expected behavior)
// but it shouldn't completely trust the bad measurement
assert!(
after_outlier[0] > before_outlier[0],
"Filter should be affected by outlier"
);
assert!(
after_outlier[0] < 1000.0,
"Filter shouldn't jump all the way to outlier"
);
// Continue with normal measurements - should recover
for i in 22..=100 {
ekf.predict(&[], dt);
ekf.update(&[i as f64 * dt]).unwrap();
}
let recovered = ekf.estimate();
let expected = 100.0 * dt;
assert!(
(recovered[0] - expected).abs() < 4.0,
"Failed to recover from outlier: {} vs {}",
recovered[0],
expected
);
}
#[test]
fn test_ekf_handles_missing_measurements() {
// Run two filters: one with every measurement, one with sparse measurements
let mut ekf_dense = ekf::ExtendedKalmanFilter::new(
[0.0, 1.0],
[0.5, 0.3],
ConstantVelocity,
PositionSensor,
);
let mut ekf_sparse = ekf::ExtendedKalmanFilter::new(
[0.0, 1.0],
[0.5, 0.3],
ConstantVelocity,
PositionSensor,
);
let dt = 0.1;
for i in 1..=100 {
// Both filters predict
ekf_dense.predict(&[], dt);
ekf_sparse.predict(&[], dt);
// Dense filter gets every measurement
ekf_dense.update(&[i as f64 * dt]).unwrap();
// Sparse filter only gets every 5th measurement
if i % 5 == 0 {
ekf_sparse.update(&[i as f64 * dt]).unwrap();
}
}
let est_dense = ekf_dense.estimate();
let est_sparse = ekf_sparse.estimate();
let unc_dense = ekf_dense.uncertainty();
let unc_sparse = ekf_sparse.uncertainty();
// Both should track reasonably well
assert!(
(est_dense[0] - 10.0).abs() < 1.0,
"Dense tracking failed: {}",
est_dense[0]
);
assert!(
(est_sparse[0] - 10.0).abs() < 2.0,
"Sparse tracking failed: {}",
est_sparse[0]
);
// Sparse measurements should result in higher uncertainty
assert!(
unc_sparse[0] > unc_dense[0],
"Sparse should have higher uncertainty: {} vs {}",
unc_sparse[0],
unc_dense[0]
);
}
#[test]
fn test_ekf_numerical_stability_long_run() {
let mut ekf = ekf::ExtendedKalmanFilter::new(
[0.0, 1.0],
[1.0, 0.5],
ConstantVelocity,
PositionSensor,
);
let dt = 0.01;
// Run for a very long time
for i in 1..=10000 {
ekf.predict(&[], dt);
ekf.update(&[i as f64 * dt]).unwrap();
let est = ekf.estimate();
let unc = ekf.uncertainty();
// Check for NaN or Inf
assert!(est[0].is_finite(), "NaN/Inf in state at step {}", i);
assert!(est[1].is_finite(), "NaN/Inf in velocity at step {}", i);
assert!(unc[0].is_finite(), "NaN/Inf in uncertainty at step {}", i);
assert!(unc[0] > 0.0, "Uncertainty went non-positive at step {}", i);
}
}
#[test]
fn test_pf_recovers_from_low_diversity() {
use rand::SeedableRng;
use rand_chacha::ChaCha8Rng;
// Use tight likelihood that will cause weight collapse
struct TightSensor;
impl particle::MeasurementModel<[f64; 2], [f64; 1]> for TightSensor {
fn likelihood(&self, state: &[f64; 2], measurement: &[f64; 1]) -> f64 {
let error = state[0] - measurement[0];
(-0.5 * (error / 0.1).powi(2)).exp() // Very tight
}
}
let initial = UniformInitial {
center: [0.0, 1.0],
spread: 1.0,
};
let mut pf = particle::ParticleFilter::new(
initial,
particle::Config {
num_particles: 200,
resample_threshold: 0.5,
},
SimpleMotion,
TightSensor,
ChaCha8Rng::seed_from_u64(123),
);
// Run many steps - resampling should keep filter healthy
for i in 1..=100 {
pf.predict(&[], 0.1);
pf.update(&[i as f64 * 0.1]).unwrap();
// Diversity should stay reasonable due to resampling
let diversity = pf.particle_diversity();
assert!(
diversity > 0.1,
"Diversity too low at step {}: {}",
i,
diversity
);
}
}
#[test]
fn test_pf_different_seeds_converge_similarly() {
use rand::SeedableRng;
use rand_chacha::ChaCha8Rng;
let mut estimates: Vec<f64> = Vec::new();
for seed in [1, 42, 123, 999, 314159] {
let initial = UniformInitial {
center: [0.0, 1.0],
spread: 2.0,
};
let mut pf = particle::ParticleFilter::new(
initial,
particle::Config {
num_particles: 500,
resample_threshold: 0.5,
},
SimpleMotion,
SimpleSensor,
ChaCha8Rng::seed_from_u64(seed),
);
for i in 1..=50 {
pf.predict(&[], 0.1);
pf.update(&[i as f64 * 0.1]).unwrap();
}
estimates.push(pf.estimate()[0]);
}
// All estimates should be reasonably close
let mean: f64 = estimates.iter().sum::<f64>() / estimates.len() as f64;
let max_deviation = estimates
.iter()
.map(|e| (e - mean).abs())
.fold(0.0, f64::max);
assert!(
max_deviation < 0.5,
"Too much variance between seeds: estimates = {:?}",
estimates
);
}
}
mod comparison_tests {
//! Compare EKF and Particle Filter on the same problems
use super::*;
use rand::SeedableRng;
use rand_chacha::ChaCha8Rng;
#[test]
fn test_ekf_and_pf_agree_on_linear_problem() {
// For linear problems, both should give similar results
// EKF setup
let mut ekf = ekf::ExtendedKalmanFilter::new(
[0.0, 1.0],
[1.0, 0.5],
ConstantVelocity,
PositionSensor,
);
// Matching PF setup
let initial = UniformInitial {
center: [0.0, 1.0],
spread: 2.0, // Roughly matches EKF uncertainty
};
let mut pf = particle::ParticleFilter::new(
initial,
particle::Config {
num_particles: 2000,
resample_threshold: 0.5,
},
SimpleMotion,
SimpleSensor,
ChaCha8Rng::seed_from_u64(42),
);
let dt = 0.1;
// Run both with identical measurements
for i in 1..=50 {
let measurement = i as f64 * dt;
ekf.predict(&[], dt);
ekf.update(&[measurement]).unwrap();
pf.predict(&[], dt);
pf.update(&[measurement]).unwrap();
}
let ekf_est = ekf.estimate();
let pf_est = pf.estimate();
// Should agree to within PF sampling error
assert!(
(ekf_est[0] - pf_est[0]).abs() < 0.3,
"Position disagreement: EKF={}, PF={}",
ekf_est[0],
pf_est[0]
);
assert!(
(ekf_est[1] - pf_est[1]).abs() < 0.2,
"Velocity disagreement: EKF={}, PF={}",
ekf_est[1],
pf_est[1]
);
}
}
mod step_method_tests {
//! Test the convenience step() method
use super::*;
#[test]
fn test_step_equals_predict_update() {
let mut ekf1 = ekf::ExtendedKalmanFilter::new(
[0.0, 1.0],
[1.0, 0.5],
ConstantVelocity,
PositionSensor,
);
let mut ekf2 = ekf::ExtendedKalmanFilter::new(
[0.0, 1.0],
[1.0, 0.5],
ConstantVelocity,
PositionSensor,
);
let dt = 0.1;
let measurement = [0.15];
// Method 1: step()
let est1 = ekf1.step(&[], &measurement, dt).unwrap();
// Method 2: predict() then update()
ekf2.predict(&[], dt);
ekf2.update(&measurement).unwrap();
let est2 = ekf2.estimate();
assert_eq!(est1, est2);
}
}
mod edge_case_tests {
//! Test edge cases and boundary conditions
use super::*;
use rand::SeedableRng;
use rand_chacha::ChaCha8Rng;
#[test]
fn test_ekf_zero_dt() {
let mut ekf = ekf::ExtendedKalmanFilter::new(
[5.0, 2.0],
[1.0, 0.5],
ConstantVelocity,
PositionSensor,
);
let before = ekf.estimate();
ekf.predict(&[], 0.0);
let after = ekf.estimate();
// State should be unchanged with dt=0
assert_eq!(before, after);
}
#[test]
fn test_ekf_very_small_dt() {
let mut ekf = ekf::ExtendedKalmanFilter::new(
[0.0, 1.0],
[1.0, 0.5],
ConstantVelocity,
PositionSensor,
);
let dt = 1e-10;
for _ in 0..1000 {
ekf.predict(&[], dt);
ekf.update(&[0.0]).unwrap();
let est = ekf.estimate();
assert!(est[0].is_finite());
assert!(est[1].is_finite());
}
}
#[test]
fn test_ekf_large_dt() {
let mut ekf = ekf::ExtendedKalmanFilter::new(
[0.0, 1.0],
[1.0, 0.5],
ConstantVelocity,
PositionSensor,
);
ekf.predict(&[], 1000.0);
let est = ekf.estimate();
// Should predict position = 1000 (velocity * dt)
assert!((est[0] - 1000.0).abs() < 1.0);
assert!(est[0].is_finite());
}
#[test]
fn test_pf_single_particle() {
// Degenerate case: just one particle
struct SingleParticle;
impl rand::distr::Distribution<[f64; 2]> for SingleParticle {
fn sample<R: rand::Rng + ?Sized>(&self, _: &mut R) -> [f64; 2] {
[0.0, 1.0]
}
}
let mut pf = particle::ParticleFilter::new(
SingleParticle,
particle::Config {
num_particles: 1,
resample_threshold: 0.5,
},
SimpleMotion,
SimpleSensor,
ChaCha8Rng::seed_from_u64(1),
);
assert_eq!(pf.num_particles(), 1);
// Should still work (though not well)
pf.predict(&[], 0.1);
pf.update(&[0.1]).unwrap();
let est = pf.estimate();
assert!(est[0].is_finite());
}
#[test]
fn test_pf_many_particles() {
let initial = UniformInitial {
center: [0.0, 1.0],
spread: 1.0,
};
let mut pf = particle::ParticleFilter::new(
initial,
particle::Config {
num_particles: 10000,
resample_threshold: 0.5,
},
SimpleMotion,
SimpleSensor,
ChaCha8Rng::seed_from_u64(42),
);
assert_eq!(pf.num_particles(), 10000);
for i in 1..=20 {
pf.predict(&[], 0.1);
pf.update(&[i as f64 * 0.1]).unwrap();
}
let est = pf.estimate();
// With many particles, should be very accurate
assert!((est[0] - 2.0).abs() < 0.2);
}
}
mod control_input_tests {
//! Test systems with non-trivial control inputs
use super::*;
use rand::SeedableRng;
use rand_chacha::ChaCha8Rng;
/// Accelerating object: state = [position, velocity], control = [acceleration]
struct AcceleratingMotion;
impl ekf::ProcessModel<2, 1> for AcceleratingMotion {
fn predict_next(&self, state: &[f64; 2], control: &[f64; 1], dt: f64) -> [f64; 2] {
let [pos, vel] = *state;
let [acc] = *control;
[pos + vel * dt + 0.5 * acc * dt * dt, vel + acc * dt]
}
fn process_noise_std(&self) -> [f64; 2] {
[0.05, 0.02]
}
}
#[test]
fn test_ekf_with_acceleration_control() {
let mut ekf = ekf::ExtendedKalmanFilter::new(
[0.0, 0.0],
[0.5, 0.3],
AcceleratingMotion,
PositionSensor,
);
let dt = 0.1;
let acceleration = 2.0;
// Simulate accelerating motion: x = 0.5 * a * t^2
for i in 1..=50 {
let t = i as f64 * dt;
let true_pos = 0.5 * acceleration * t * t;
let true_vel = acceleration * t;
ekf.predict(&[acceleration], dt);
ekf.update(&[true_pos]).unwrap();
let est = ekf.estimate();
// Allow some error for filter lag
if i > 10 {
assert!(
(est[0] - true_pos).abs() < 0.5,
"Position error at t={}: {} vs {}",
t,
est[0],
true_pos
);
assert!(
(est[1] - true_vel).abs() < 0.5,
"Velocity error at t={}: {} vs {}",
t,
est[1],
true_vel
);
}
}
}
#[test]
fn test_ekf_varying_control_inputs() {
let mut ekf = ekf::ExtendedKalmanFilter::new(
[0.0, 0.0],
[1.0, 0.5],
AcceleratingMotion,
PositionSensor,
);
let dt = 0.1;
let mut true_pos = 0.0;
let mut true_vel = 0.0;
// Alternating acceleration pattern
for i in 1..=100 {
let acc = if (i / 10) % 2 == 0 { 1.0 } else { -1.0 };
true_pos += true_vel * dt + 0.5 * acc * dt * dt;
true_vel += acc * dt;
ekf.predict(&[acc], dt);
ekf.update(&[true_pos]).unwrap();
}
let est = ekf.estimate();
assert!(
(est[0] - true_pos).abs() < 1.0,
"Position: {} vs {}",
est[0],
true_pos
);
}
#[test]
fn test_pf_with_control_inputs() {
struct AccelMotionPF;
impl particle::ProcessModel<[f64; 2], [f64; 1]> for AccelMotionPF {
fn sample_next<R: rand::Rng>(
&self,
state: &[f64; 2],
control: &[f64; 1],
dt: f64,
rng: &mut R,
) -> [f64; 2] {
use rand_distr::{Distribution, StandardNormal};
let [pos, vel] = *state;
let [acc] = *control;
let n1: f64 = StandardNormal.sample(rng);
let n2: f64 = StandardNormal.sample(rng);
[
pos + vel * dt + 0.5 * acc * dt * dt + n1 * 0.05,
vel + acc * dt + n2 * 0.02,
]
}
}
let initial = UniformInitial {
center: [0.0, 0.0],
spread: 1.0,
};
let mut pf = particle::ParticleFilter::new(
initial,
particle::Config {
num_particles: 500,
resample_threshold: 0.5,
},
AccelMotionPF,
SimpleSensor,
ChaCha8Rng::seed_from_u64(42),
);
let dt = 0.1;
let acc = 2.0;
for i in 1..=30 {
let t = i as f64 * dt;
let true_pos = 0.5 * acc * t * t;
pf.predict(&[acc], dt);
pf.update(&[true_pos]).unwrap();
}
let est = pf.estimate();
let final_true_pos = 0.5 * acc * (30.0 * dt).powi(2);
assert!(
(est[0] - final_true_pos).abs() < 0.5,
"PF position: {} vs {}",
est[0],
final_true_pos
);
}
}
mod higher_dimensional_tests {
//! Test with larger state spaces
use super::*;
use rand::SeedableRng;
use rand_chacha::ChaCha8Rng;
/// 4D state: [x, y, vx, vy] - 2D position and velocity
struct Motion2D;
impl ekf::ProcessModel<4, 0> for Motion2D {
fn predict_next(&self, state: &[f64; 4], _: &[f64; 0], dt: f64) -> [f64; 4] {
[
state[0] + state[2] * dt,
state[1] + state[3] * dt,
state[2],
state[3],
]
}
fn process_noise_std(&self) -> [f64; 4] {
[0.1, 0.1, 0.05, 0.05]
}
}
struct PositionSensor2D;
impl ekf::MeasurementModel<4, 2> for PositionSensor2D {
fn expected_measurement(&self, state: &[f64; 4]) -> [f64; 2] {
[state[0], state[1]]
}
fn measurement_noise_std(&self) -> [f64; 2] {
[0.5, 0.5]
}
}
#[test]
fn test_ekf_4d_tracking() {
let mut ekf = ekf::ExtendedKalmanFilter::new(
[0.0, 0.0, 1.0, 0.5], // Moving diagonally
[1.0, 1.0, 0.5, 0.5],
Motion2D,
PositionSensor2D,
);
let dt = 0.1;
let vx = 1.0;
let vy = 0.5;
for i in 1..=50 {
let t = i as f64 * dt;
ekf.predict(&[], dt);
ekf.update(&[vx * t, vy * t]).unwrap();
}
let est = ekf.estimate();
let final_t = 50.0 * dt;
assert!((est[0] - vx * final_t).abs() < 0.5, "X position error");
assert!((est[1] - vy * final_t).abs() < 0.5, "Y position error");
assert!((est[2] - vx).abs() < 0.3, "X velocity error");
assert!((est[3] - vy).abs() < 0.3, "Y velocity error");
}
/// 6D state: [x, y, z, vx, vy, vz] - 3D position and velocity
struct Motion3D;
impl ekf::ProcessModel<6, 0> for Motion3D {
fn predict_next(&self, state: &[f64; 6], _: &[f64; 0], dt: f64) -> [f64; 6] {
[
state[0] + state[3] * dt,
state[1] + state[4] * dt,
state[2] + state[5] * dt,
state[3],
state[4],
state[5],
]
}
fn process_noise_std(&self) -> [f64; 6] {
[0.1, 0.1, 0.1, 0.05, 0.05, 0.05]
}
}
struct PositionSensor3D;
impl ekf::MeasurementModel<6, 3> for PositionSensor3D {
fn expected_measurement(&self, state: &[f64; 6]) -> [f64; 3] {
[state[0], state[1], state[2]]
}
fn measurement_noise_std(&self) -> [f64; 3] {
[0.5, 0.5, 0.5]
}
}
#[test]
fn test_ekf_6d_tracking() {
let mut ekf = ekf::ExtendedKalmanFilter::new(
[0.0, 0.0, 0.0, 1.0, 2.0, -0.5],
[1.0, 1.0, 1.0, 0.5, 0.5, 0.5],
Motion3D,
PositionSensor3D,
);
let dt = 0.1;
let velocity = [1.0, 2.0, -0.5];
for i in 1..=50 {
let t = i as f64 * dt;
ekf.predict(&[], dt);
ekf.update(&[velocity[0] * t, velocity[1] * t, velocity[2] * t])
.unwrap();
}
let est = ekf.estimate();
let final_t = 50.0 * dt;
for dim in 0..3 {
assert!(
(est[dim] - velocity[dim] * final_t).abs() < 0.5,
"Dimension {} position error",
dim
);
assert!(
(est[dim + 3] - velocity[dim]).abs() < 0.3,
"Dimension {} velocity error",
dim
);
}
}
#[test]
fn test_pf_4d_tracking() {
struct Motion2DPF;
impl particle::ProcessModel<[f64; 4], [f64; 0]> for Motion2DPF {
fn sample_next<R: rand::Rng>(
&self,
state: &[f64; 4],
_: &[f64; 0],
dt: f64,
rng: &mut R,
) -> [f64; 4] {
use rand_distr::{Distribution, StandardNormal};
let n1: f64 = StandardNormal.sample(rng);
let n2: f64 = StandardNormal.sample(rng);
let n3: f64 = StandardNormal.sample(rng);
let n4: f64 = StandardNormal.sample(rng);
[
state[0] + state[2] * dt + n1 * 0.1,
state[1] + state[3] * dt + n2 * 0.1,
state[2] + n3 * 0.05,
state[3] + n4 * 0.05,
]
}
}
struct Sensor2DPF;
impl particle::MeasurementModel<[f64; 4], [f64; 2]> for Sensor2DPF {
fn likelihood(&self, state: &[f64; 4], measurement: &[f64; 2]) -> f64 {
let ex = state[0] - measurement[0];
let ey = state[1] - measurement[1];
(-0.5 * (ex * ex + ey * ey) / 0.25).exp()
}
}
struct Initial4D;
impl rand::distr::Distribution<[f64; 4]> for Initial4D {
fn sample<R: rand::Rng + ?Sized>(&self, rng: &mut R) -> [f64; 4] {
let r1: f64 = rng.random();
let r2: f64 = rng.random();
let r3: f64 = rng.random();
let r4: f64 = rng.random();
[
(r1 - 0.5) * 2.0,
(r2 - 0.5) * 2.0,
1.0 + (r3 - 0.5) * 0.5,
0.5 + (r4 - 0.5) * 0.5,
]
}
}
let mut pf = particle::ParticleFilter::new(
Initial4D,
particle::Config {
num_particles: 1000,
resample_threshold: 0.5,
},
Motion2DPF,
Sensor2DPF,
ChaCha8Rng::seed_from_u64(42),
);
let dt = 0.1;
for i in 1..=30 {
let t = i as f64 * dt;
pf.predict(&[], dt);
pf.update(&[t, 0.5 * t]).unwrap();
}
let est = pf.estimate();
let final_t = 30.0 * dt;
assert!((est[0] - final_t).abs() < 0.5, "X position");
assert!((est[1] - 0.5 * final_t).abs() < 0.5, "Y position");
}
}
mod multiple_update_tests {
//! Test multiple measurement updates per prediction
use super::*;
#[test]
fn test_ekf_multiple_sensors() {
// Two sensors measuring the same position
struct AccurateSensor;
impl ekf::MeasurementModel<2, 1> for AccurateSensor {
fn expected_measurement(&self, state: &[f64; 2]) -> [f64; 1] {
[state[0]]
}
fn measurement_noise_std(&self) -> [f64; 1] {
[0.1]
}
}
struct NoisySensor;
impl ekf::MeasurementModel<2, 1> for NoisySensor {
fn expected_measurement(&self, state: &[f64; 2]) -> [f64; 1] {
[state[0]]
}
fn measurement_noise_std(&self) -> [f64; 1] {
[1.0]
}
}
let mut ekf_accurate = ekf::ExtendedKalmanFilter::new(
[0.0, 1.0],
[1.0, 0.5],
ConstantVelocity,
AccurateSensor,
);
let mut ekf_noisy =
ekf::ExtendedKalmanFilter::new([0.0, 1.0], [1.0, 0.5], ConstantVelocity, NoisySensor);
let dt = 0.1;
for i in 1..=50 {
let true_pos = i as f64 * dt;
ekf_accurate.predict(&[], dt);
ekf_accurate.update(&[true_pos]).unwrap();
ekf_noisy.predict(&[], dt);
ekf_noisy.update(&[true_pos]).unwrap();
}
// Accurate sensor should yield lower uncertainty
let unc_accurate = ekf_accurate.uncertainty();
let unc_noisy = ekf_noisy.uncertainty();
assert!(
unc_accurate[0] < unc_noisy[0],
"Accurate sensor should give lower uncertainty: {} vs {}",
unc_accurate[0],
unc_noisy[0]
);
}
#[test]
fn test_ekf_multiple_updates_per_predict() {
let mut ekf = ekf::ExtendedKalmanFilter::new(
[0.0, 1.0],
[1.0, 0.5],
ConstantVelocity,
PositionSensor,
);
let dt = 0.1;
for i in 1..=20 {
let true_pos = i as f64 * dt;
ekf.predict(&[], dt);
// Multiple updates with slightly different measurements
ekf.update(&[true_pos - 0.1]).unwrap();
ekf.update(&[true_pos + 0.1]).unwrap();
ekf.update(&[true_pos]).unwrap();
}
let est = ekf.estimate();
let unc = ekf.uncertainty();
// Should track well
assert!((est[0] - 2.0).abs() < 0.3);
// Multiple updates should reduce uncertainty further
assert!(
unc[0] < 0.3,
"Uncertainty should be low with multiple updates"
);
}
}
mod reset_and_initialization_tests {
//! Test reset() and with_covariance() methods
use super::*;
#[test]
fn test_ekf_reset() {
let mut ekf = ekf::ExtendedKalmanFilter::new(
[0.0, 1.0],
[1.0, 0.5],
ConstantVelocity,
PositionSensor,
);
// Run for a while
for i in 1..=20 {
ekf.predict(&[], 0.1);
ekf.update(&[i as f64 * 0.1]).unwrap();
}
let before_reset = ekf.estimate();
assert!(before_reset[0] > 1.0); // Moved from origin
// Reset to new state
ekf.reset([100.0, 5.0], [2.0, 1.0]);
let after_reset = ekf.estimate();
let unc_after = ekf.uncertainty();
assert_eq!(after_reset[0], 100.0);
assert_eq!(after_reset[1], 5.0);
assert!((unc_after[0] - 2.0).abs() < 0.01);
assert!((unc_after[1] - 1.0).abs() < 0.01);
}
#[test]
fn test_ekf_with_covariance() {
// Non-diagonal initial covariance (correlated uncertainty)
let initial_cov = [[1.0, 0.5], [0.5, 1.0]];
let ekf = ekf::ExtendedKalmanFilter::with_covariance(
[0.0, 1.0],
initial_cov,
ConstantVelocity,
PositionSensor,
);
let unc = ekf.uncertainty();
assert!((unc[0] - 1.0).abs() < 0.01);
assert!((unc[1] - 1.0).abs() < 0.01);
}
#[test]
fn test_ekf_reset_recovers_tracking() {
let mut ekf = ekf::ExtendedKalmanFilter::new(
[0.0, 1.0],
[1.0, 0.5],
ConstantVelocity,
PositionSensor,
);
// Get into a bad state by giving wrong measurements
for _ in 0..20 {
ekf.predict(&[], 0.1);
ekf.update(&[1000.0]).unwrap(); // Way off
}
// Reset to correct state
ekf.reset([0.0, 1.0], [0.5, 0.3]);
// Should track correctly again
for i in 1..=20 {
ekf.predict(&[], 0.1);
ekf.update(&[i as f64 * 0.1]).unwrap();
}
let est = ekf.estimate();
assert!((est[0] - 2.0).abs() < 0.5);
}
}
mod jacobian_tests {
//! Test numerical vs analytical Jacobians
use super::*;
use ekf::ProcessModel;
#[test]
fn test_numerical_jacobian_accuracy() {
// Test on a known nonlinear function
fn nonlinear(x: &[f64; 2]) -> [f64; 2] {
[x[0].powi(2) + x[1].sin(), x[0] * x[1].exp()]
}
let point = [1.0, 0.5];
let jac = ekf::numerical_jacobian(nonlinear, &point);
// Analytical derivatives:
// df1/dx0 = 2*x0 = 2.0
// df1/dx1 = cos(x1) ≈ 0.8776
// df2/dx0 = exp(x1) ≈ 1.6487
// df2/dx1 = x0 * exp(x1) ≈ 1.6487
assert!((jac[0][0] - 2.0).abs() < 1e-5, "df1/dx0");
assert!((jac[1][0] - 0.5_f64.cos()).abs() < 1e-5, "df1/dx1");
assert!((jac[0][1] - 0.5_f64.exp()).abs() < 1e-5, "df2/dx0");
assert!((jac[1][1] - 0.5_f64.exp()).abs() < 1e-5, "df2/dx1");
}
#[test]
fn test_analytical_matches_numerical() {
struct AnalyticalModel;
impl ekf::ProcessModel<2, 0> for AnalyticalModel {
fn predict_next(&self, state: &[f64; 2], _: &[f64; 0], dt: f64) -> [f64; 2] {
// Nonlinear: x' = x + v*dt, v' = v * cos(x)
[state[0] + state[1] * dt, state[1] * state[0].cos()]
}
fn process_noise_std(&self) -> [f64; 2] {
[0.1, 0.1]
}
fn prediction_sensitivity(
&self,
state: &[f64; 2],
_: &[f64; 0],
dt: f64,
) -> [[f64; 2]; 2] {
// Analytical Jacobian
[
[1.0, -state[1] * state[0].sin()], // d/dx0 of [x', v']
[dt, state[0].cos()], // d/dx1 of [x', v']
]
}
}
let state = [0.5, 1.0];
let dt = 0.1;
// Compute numerical Jacobian
let numerical =
ekf::numerical_jacobian(|s| AnalyticalModel.predict_next(s, &[], dt), &state);
// Get analytical
let analytical = AnalyticalModel.prediction_sensitivity(&state, &[], dt);
for i in 0..2 {
for j in 0..2 {
assert!(
(numerical[i][j] - analytical[i][j]).abs() < 1e-5,
"Mismatch at [{},{}]: numerical={}, analytical={}",
i,
j,
numerical[i][j],
analytical[i][j]
);
}
}
}
}
mod particle_filter_advanced_tests {
//! Additional particle filter tests
use super::*;
use rand::SeedableRng;
use rand_chacha::ChaCha8Rng;
#[test]
fn test_pf_best_particle() {
let initial = UniformInitial {
center: [0.0, 1.0],
spread: 0.5,
};
let mut pf = particle::ParticleFilter::new(
initial,
particle::Config {
num_particles: 100,
resample_threshold: 0.5,
},
SimpleMotion,
SimpleSensor,
ChaCha8Rng::seed_from_u64(42),
);
// After measurement, best particle should be close to measurement
pf.predict(&[], 0.1);
pf.update(&[0.1]).unwrap();
let best = pf.best_particle().unwrap();
let estimate = pf.estimate();
// Best particle should be reasonable
assert!(best[0].is_finite());
// Estimate should be close to best particle (though not necessarily identical)
assert!((best[0] - estimate[0]).abs() < 0.5);
}
#[test]
fn test_pf_from_particles() {
// Initialize from explicit particle list
let particles = vec![[0.0, 1.0], [0.1, 1.0], [-0.1, 1.0], [0.0, 1.1], [0.0, 0.9]];
let mut pf = particle::ParticleFilter::from_particles(
particles.clone(),
particle::Config {
num_particles: 5,
resample_threshold: 0.5,
},
SimpleMotion,
SimpleSensor,
ChaCha8Rng::seed_from_u64(42),
);
assert_eq!(pf.num_particles(), 5);
// Initial estimate should be mean of particles
let est = pf.estimate();
let mean_x: f64 = particles.iter().map(|p| p[0]).sum::<f64>() / 5.0;
let mean_v: f64 = particles.iter().map(|p| p[1]).sum::<f64>() / 5.0;
assert!((est[0] - mean_x).abs() < 0.01);
assert!((est[1] - mean_v).abs() < 0.01);
// Should still work after update
pf.predict(&[], 0.1);
pf.update(&[0.1]).unwrap();
let new_est = pf.estimate();
assert!(new_est[0].is_finite());
}
#[test]
fn test_pf_resampling_thresholds() {
// Test with different resampling thresholds
for threshold in [0.1, 0.5, 0.9] {
let initial = UniformInitial {
center: [0.0, 1.0],
spread: 1.0,
};
let mut pf = particle::ParticleFilter::new(
initial,
particle::Config {
num_particles: 200,
resample_threshold: threshold,
},
SimpleMotion,
SimpleSensor,
ChaCha8Rng::seed_from_u64(42),
);
for i in 1..=30 {
pf.predict(&[], 0.1);
pf.update(&[i as f64 * 0.1]).unwrap();
}
let est = pf.estimate();
assert!(
(est[0] - 3.0).abs() < 0.5,
"Threshold {} failed: estimate = {}",
threshold,
est[0]
);
}
}
#[test]
fn test_pf_particle_diversity_reporting() {
let initial = UniformInitial {
center: [0.0, 1.0],
spread: 2.0,
};
let mut pf = particle::ParticleFilter::new(
initial,
particle::Config {
num_particles: 500,
resample_threshold: 0.5,
},
SimpleMotion,
SimpleSensor,
ChaCha8Rng::seed_from_u64(42),
);
// Initial diversity should be high (all weights equal)
let initial_diversity = pf.particle_diversity();
assert!(initial_diversity > 0.9, "Initial diversity should be ~1.0");
// After update, diversity typically drops
pf.predict(&[], 0.1);
pf.update(&[0.1]).unwrap();
let diversity = pf.particle_diversity();
assert!(
diversity > 0.0 && diversity <= 1.0,
"Diversity should be in (0, 1]: {}",
diversity
);
}
}
mod stationary_tracking_tests {
//! Test tracking of stationary or slow-moving objects
use super::*;
#[test]
fn test_ekf_stationary_object() {
let mut ekf = ekf::ExtendedKalmanFilter::new(
[5.0, 0.0], // Stationary at position 5
[1.0, 0.5],
ConstantVelocity,
PositionSensor,
);
let dt = 0.1;
// Object stays at position 5
for _ in 0..50 {
ekf.predict(&[], dt);
ekf.update(&[5.0]).unwrap();
}
let est = ekf.estimate();
assert!((est[0] - 5.0).abs() < 0.2, "Position should stay at 5");
assert!(est[1].abs() < 0.2, "Velocity should be ~0");
}
#[test]
fn test_ekf_object_stops() {
let mut ekf = ekf::ExtendedKalmanFilter::new(
[0.0, 1.0], // Moving
[0.5, 0.5],
ConstantVelocity,
PositionSensor,
);
let dt = 0.1;
// First phase: moving
for i in 1..=20 {
ekf.predict(&[], dt);
ekf.update(&[i as f64 * dt]).unwrap();
}
// Second phase: stopped at position 2
for _ in 0..50 {
ekf.predict(&[], dt);
ekf.update(&[2.0]).unwrap();
}
let est = ekf.estimate();
assert!((est[0] - 2.0).abs() < 0.3, "Position should converge to 2");
assert!(est[1].abs() < 0.3, "Velocity should converge to 0");
}
}
mod observability_tests {
//! Test behavior when state is not fully observable
use super::*;
/// Sensor that only measures position, not velocity
struct PositionOnlySensor;
impl ekf::MeasurementModel<2, 1> for PositionOnlySensor {
fn expected_measurement(&self, state: &[f64; 2]) -> [f64; 1] {
[state[0]]
}
fn measurement_noise_std(&self) -> [f64; 1] {
[0.5]
}
}
#[test]
fn test_velocity_inferred_from_position_changes() {
let mut ekf = ekf::ExtendedKalmanFilter::new(
[0.0, 0.0], // Unknown velocity initially
[0.5, 2.0], // High velocity uncertainty
ConstantVelocity,
PositionOnlySensor,
);
let dt = 0.1;
let true_velocity = 1.5;
// Feed position measurements and velocity should be inferred
for i in 1..=50 {
ekf.predict(&[], dt);
ekf.update(&[true_velocity * i as f64 * dt]).unwrap();
}
let est = ekf.estimate();
assert!(
(est[1] - true_velocity).abs() < 0.3,
"Velocity should be inferred: {} vs {}",
est[1],
true_velocity
);
}
#[test]
fn test_unobservable_state_maintains_uncertainty() {
// If we only measure sum of states, individual components are unobservable
struct SumSensor;
impl ekf::MeasurementModel<2, 1> for SumSensor {
fn expected_measurement(&self, state: &[f64; 2]) -> [f64; 1] {
[state[0] + state[1]] // Only measures sum
}
fn measurement_noise_std(&self) -> [f64; 1] {
[0.1]
}
}
struct Identity;
impl ekf::ProcessModel<2, 0> for Identity {
fn predict_next(&self, state: &[f64; 2], _: &[f64; 0], _dt: f64) -> [f64; 2] {
*state // State doesn't change
}
fn process_noise_std(&self) -> [f64; 2] {
[0.01, 0.01]
}
}
let mut ekf = ekf::ExtendedKalmanFilter::new([0.0, 0.0], [1.0, 1.0], Identity, SumSensor);
// Measure sum = 1.0 repeatedly
for _ in 0..100 {
ekf.predict(&[], 0.1);
ekf.update(&[1.0]).unwrap();
}
let est = ekf.estimate();
// Sum should converge to 1.0
assert!(
(est[0] + est[1] - 1.0).abs() < 0.1,
"Sum should be 1.0: {} + {} = {}",
est[0],
est[1],
est[0] + est[1]
);
// But individual uncertainties won't collapse as much as if
// we could observe them directly
let unc = ekf.uncertainty();
// Note: Due to process noise and observability, individual uncertainties
// should still be reduced somewhat but remain meaningful
assert!(unc[0] > 0.01 && unc[1] > 0.01);
}
}