#![allow(clippy::needless_range_loop, clippy::many_single_char_names)]
use crate::srif::{Srif, SrifError};
pub trait ResidualModel<const N: usize> {
fn residuals(&self, x: &[f64; N], emit: &mut dyn FnMut(&[f64; N], f64, f64));
}
#[derive(Debug, Clone, Copy)]
pub struct GaussNewton {
pub max_iterations: u32,
pub cost_tolerance: f64,
pub step_tolerance: f64,
pub initial_lambda: f64,
pub absolute_cost_floor: f64,
}
impl Default for GaussNewton {
fn default() -> Self {
Self {
max_iterations: 50,
cost_tolerance: 1e-12,
step_tolerance: 1e-12,
initial_lambda: 1e-3,
absolute_cost_floor: 1e-18,
}
}
}
pub type Estimate<const N: usize> = ([f64; N], [[f64; N]; N], GaussNewtonReport);
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct GaussNewtonReport {
pub iterations: u32,
pub cost: f64,
pub converged: bool,
}
impl GaussNewton {
pub fn solve<const N: usize, M: ResidualModel<N>>(
&self,
model: &M,
x0: &[f64; N],
) -> Result<Estimate<N>, SrifError> {
let mut x = *x0;
let mut lambda = self.initial_lambda;
let mut cost = weighted_cost(model, &x);
let mut converged = false;
let mut iterations = 0;
while iterations < self.max_iterations {
iterations += 1;
let mut srif = Srif::<N>::new();
let mut information_diagonal = [0.0; N];
model.residuals(&x, &mut |h, r, w| {
srif.add_row(h, -r, w);
for i in 0..N {
information_diagonal[i] += w * h[i] * h[i];
}
});
if lambda > 0.0 {
for i in 0..N {
let mut e = [0.0; N];
e[i] = 1.0;
srif.add_row(&e, 0.0, lambda * information_diagonal[i]);
}
}
let delta = srif.solve()?;
let mut trial = x;
for i in 0..N {
trial[i] += delta[i];
}
let trial_cost = weighted_cost(model, &trial);
if trial_cost < cost {
let step_norm = norm(&delta) / norm(&x).max(1.0);
let cost_drop = (cost - trial_cost) / cost.max(f64::MIN_POSITIVE);
x = trial;
cost = trial_cost;
lambda = (lambda * 0.3).max(1e-12);
if step_norm < self.step_tolerance
|| cost_drop < self.cost_tolerance
|| cost <= self.absolute_cost_floor
{
converged = true;
break;
}
} else {
lambda *= 10.0;
if lambda >= 1e12 {
converged = true;
break;
}
}
}
let mut srif = Srif::<N>::new();
model.residuals(&x, &mut |h, _r, w| srif.add_row(h, 0.0, w));
let cov = srif.covariance()?;
Ok((
x,
cov,
GaussNewtonReport {
iterations,
cost,
converged,
},
))
}
}
fn weighted_cost<const N: usize, M: ResidualModel<N>>(model: &M, x: &[f64; N]) -> f64 {
let mut c = 0.0;
model.residuals(x, &mut |_h, r, w| c += w * r * r);
c
}
fn norm<const N: usize>(v: &[f64; N]) -> f64 {
let mut s = 0.0;
for &x in v {
s += x * x;
}
libm::sqrt(s)
}