1use std::ops::{Add, Mul};
3use rand::Rng;
4
5#[derive(Clone, Debug)]
14pub struct Increment<B: Clone> {
15 pub dw: B,
16 pub dz: B,
17}
18
19pub trait State:
22 Clone + Send + Sync + 'static
23 + Add<Output = Self>
24 + Mul<f64, Output = Self>
25{
26 fn zero() -> Self;
27}
28
29impl State for f64 {
30 fn zero() -> Self { 0.0 }
31}
32
33impl<const N: usize> State for nalgebra::SVector<f64, N> {
34 fn zero() -> Self { nalgebra::SVector::zeros() }
35}
36
37pub trait NoiseIncrement: Clone + Send + Sync + 'static {
43 fn sample<R: Rng>(rng: &mut R, dt: f64) -> Increment<Self>;
44}
45
46impl NoiseIncrement for f64 {
47 fn sample<R: Rng>(rng: &mut R, dt: f64) -> Increment<Self> {
48 use rand_distr::{Distribution, Normal};
49 let normal = Normal::new(0.0_f64, 1.0).unwrap();
50 let z1 = normal.sample(rng);
51 let z2 = normal.sample(rng);
52 let dw = z1 * dt.sqrt();
53 let dz = (dt / 2.0) * dw - (dt.powi(3) / 12.0).sqrt() * z2;
54 Increment { dw, dz }
55 }
56}
57
58impl<const M: usize> NoiseIncrement for nalgebra::SVector<f64, M> {
59 fn sample<R: Rng>(rng: &mut R, dt: f64) -> Increment<Self> {
60 use rand_distr::{Distribution, Normal};
61 let normal = Normal::new(0.0_f64, 1.0).unwrap();
62 let mut dw = nalgebra::SVector::<f64, M>::zeros();
63 let mut dz = nalgebra::SVector::<f64, M>::zeros();
64 for i in 0..M {
65 let z1 = normal.sample(rng);
66 let z2 = normal.sample(rng);
67 dw[i] = z1 * dt.sqrt();
68 dz[i] = (dt / 2.0) * dw[i] - (dt.powi(3) / 12.0).sqrt() * z2;
69 }
70 Increment { dw, dz }
71 }
72}
73
74pub trait Diffusion<S: State, B: NoiseIncrement>: Send + Sync {
90 fn apply(&self, x: &S, t: f64, dw: &B) -> S;
91}
92
93impl<F: Fn(f64, f64) -> f64 + Send + Sync> Diffusion<f64, f64> for F {
95 fn apply(&self, x: &f64, t: f64, dw: &f64) -> f64 {
96 self(*x, t) * dw
97 }
98}
99
100impl<const N: usize, F> Diffusion<nalgebra::SVector<f64, N>, nalgebra::SVector<f64, N>> for F
102where
103 F: Fn(&nalgebra::SVector<f64, N>, f64) -> nalgebra::SVector<f64, N> + Send + Sync,
104{
105 fn apply(
106 &self,
107 x: &nalgebra::SVector<f64, N>,
108 t: f64,
109 dw: &nalgebra::SVector<f64, N>,
110 ) -> nalgebra::SVector<f64, N> {
111 self(x, t).component_mul(dw)
112 }
113}
114
115#[cfg(test)]
116mod tests {
117 use super::*;
118 use rand::SeedableRng;
119
120 #[test]
121 fn increment_f64_variance() {
122 let dt = 0.01_f64;
124 let n = 100_000_usize;
125 let mut rng = rand::rngs::SmallRng::seed_from_u64(0);
126 let mut dws = vec![0.0_f64; n];
127 let mut dzs = vec![0.0_f64; n];
128 for i in 0..n {
129 let inc = <f64 as NoiseIncrement>::sample(&mut rng, dt);
130 dws[i] = inc.dw;
131 dzs[i] = inc.dz;
132 }
133 let var_dw: f64 = dws.iter().map(|x| x * x).sum::<f64>() / n as f64;
134 let var_dz: f64 = dzs.iter().map(|x| x * x).sum::<f64>() / n as f64;
135 let cov: f64 = dws.iter().zip(&dzs).map(|(w, z)| w * z).sum::<f64>() / n as f64;
136 assert!((var_dw - dt).abs() / dt < 0.02, "Var[dW]={:.6} expected {:.6}", var_dw, dt);
138 let expected_var_dz = dt.powi(3) / 3.0;
140 assert!((var_dz - expected_var_dz).abs() / expected_var_dz < 0.03,
141 "Var[dZ]={:.8} expected {:.8}", var_dz, expected_var_dz);
142 let expected_cov = dt.powi(2) / 2.0;
144 assert!((cov - expected_cov).abs() / expected_cov < 0.02,
145 "Cov(dW,dZ)={:.8} expected {:.8}", cov, expected_cov);
146 }
147
148 #[test]
149 fn increment_svector_has_correct_length() {
150 let mut rng = rand::rngs::SmallRng::seed_from_u64(42);
151 let inc = <nalgebra::SVector<f64, 3> as NoiseIncrement>::sample(&mut rng, 0.01);
152 assert_eq!(inc.dw.len(), 3);
153 assert_eq!(inc.dz.len(), 3);
154 }
155}