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
use faer::{
ColRef,
prelude::Solve,
sparse::{SparseColMatRef, linalg::solvers::Lu},
};
use crate::{Config, NonLinearSystemError};
use super::Model;
#[derive(Debug)]
pub struct SuccessfulSolve {
pub iterations: usize,
}
impl Model<'_> {
#[inline(never)]
pub(crate) fn solve_gauss_newton(
&mut self,
current_values: &mut [f64],
config: Config,
) -> Result<SuccessfulSolve, NonLinearSystemError> {
let m = self.layout.total_num_residuals;
let mut global_residual = vec![0.0; m];
for this_iteration in 0..config.max_iterations {
// Assemble global residual and Jacobian
// Re-evaluate the global residual.
self.residual(current_values, &mut global_residual);
// Re-evaluate the global jacobian, write it into self.jc
self.refresh_jacobian(current_values);
// Convergence check: if the residual is within our tolerance,
// then the system is totally solved and we can return.
let largest_absolute_elem = global_residual
.iter()
.map(|x| x.abs())
.reduce(libm::fmax)
.ok_or(NonLinearSystemError::EmptySystemNotAllowed)?;
if largest_absolute_elem <= config.convergence_tolerance {
return Ok(SuccessfulSolve {
iterations: this_iteration,
});
}
/* NOTE(dr): We solve the following linear system to get the damped Gauss-Newton step d
(JᵀJ + λI) d = -Jᵀr
This involves creating a matrix A and rhs b where
A = JᵀJ + λI
b = -Jᵀr
*/
let j = SparseColMatRef::new(self.jc.sym.as_ref(), &self.jc.vals);
// TODO: Is there any way to transpose `j` and keep it in column-major?
// Converting from row- to column-major might not be necessary.
let jtj = j.transpose().to_col_major()? * j;
let a = jtj + &self.lambda_i;
let b = j.transpose() * -ColRef::from_slice(&global_residual);
// Solve linear system
let factored = Lu::try_new_with_symbolic(self.lu_symbolic.clone(), a.as_ref())?;
let d = factored.solve(&b);
assert_eq!(
d.nrows(),
current_values.len(),
"the `d` column must be the same size as the number of variables."
);
let current_inf_norm = current_values.iter().map(|v| v.abs()).fold(0.0, libm::fmax);
let step_inf_norm = d.iter().map(|d| d.abs()).reduce(libm::fmax).unwrap_or(0.0);
current_values
.iter_mut()
.zip(d.iter())
.for_each(|(curr_val, d)| {
*curr_val += d;
});
let step_threshold = config.step_tolerance * (current_inf_norm + config.step_tolerance);
// Convergence check: if `d` is small enough,
// then the system is at a local minimum. It might be inconsistent, and therefore
// its residual will never get close to zero, but this is still a good least-squares solution,
// so we can return.
if step_inf_norm <= step_threshold {
return Ok(SuccessfulSolve {
iterations: this_iteration,
});
}
}
Err(NonLinearSystemError::DidNotConverge)
}
}