#![allow(
clippy::needless_range_loop,
clippy::many_single_char_names,
clippy::similar_names
)]
use crate::consistency::normalized_squared;
use crate::robust::{self, RobustOutcome, RobustPolicy};
use crate::unscented::{UnscentedParams, centered_residuals, cholesky, sigma_points};
#[derive(Debug, Clone)]
pub struct SquareRootUnscentedKalman<const N: usize, const L: usize> {
pub x: [f64; N],
s: [[f64; N]; N],
params: UnscentedParams,
}
impl<const N: usize, const L: usize> SquareRootUnscentedKalman<N, L> {
#[must_use]
pub const fn new(
x0: [f64; N],
covariance_sqrt: [[f64; N]; N],
params: UnscentedParams,
) -> Self {
assert!(L == 2 * N + 1, "sigma-point count L must equal 2N + 1");
Self {
x: x0,
s: covariance_sqrt,
params,
}
}
#[must_use]
pub fn from_covariance(
x0: [f64; N],
p0: &[[f64; N]; N],
params: UnscentedParams,
) -> Option<Self> {
assert!(L == 2 * N + 1, "sigma-point count L must equal 2N + 1");
let s = cholesky::<N>(p0)?;
Some(Self { x: x0, s, params })
}
pub fn predict(
&mut self,
dynamics: impl Fn(&[f64; N]) -> [f64; N],
process_noise_sqrt: &[[f64; N]; N],
) {
let (x, s) =
unscented_predict::<N, L>(&self.params, &self.x, &self.s, dynamics, process_noise_sqrt);
self.x = x;
self.s = s;
}
pub fn update<const D: usize>(
&mut self,
measurement: impl Fn(&[f64; N]) -> [f64; D],
z: &[f64; D],
noise_sqrt: &[[f64; D]; D],
) {
self.update_robust(measurement, z, noise_sqrt, RobustPolicy::Plain);
}
pub fn update_robust<const D: usize>(
&mut self,
measurement: impl Fn(&[f64; N]) -> [f64; D],
z: &[f64; D],
noise_sqrt: &[[f64; D]; D],
policy: RobustPolicy,
) -> RobustOutcome {
self.fold(measurement, z, noise_sqrt, policy).0
}
pub fn update_with_log_likelihood<const D: usize>(
&mut self,
measurement: impl Fn(&[f64; N]) -> [f64; D],
z: &[f64; D],
noise_sqrt: &[[f64; D]; D],
) -> f64 {
self.fold(measurement, z, noise_sqrt, RobustPolicy::Plain).1
}
fn fold<const D: usize>(
&mut self,
measurement: impl Fn(&[f64; N]) -> [f64; D],
z: &[f64; D],
noise_sqrt: &[[f64; D]; D],
policy: RobustPolicy,
) -> (RobustOutcome, f64) {
let w = self.params.weights::<N>();
let pts = sigma_points::<N, L>(&self.x, &self.s, w.gamma);
let mut y = [[0.0; D]; L];
for i in 0..L {
y[i] = measurement(&pts[i]);
}
let (offset, resid) = centered_residuals::<D, L>(&y, &w);
let mut innov = [0.0; D];
for j in 0..D {
innov[j] = (z[j] - y[0][j]) - offset[j];
}
let nominal = innovation_factor::<D, L>(&resid, &w, noise_sqrt, 1.0);
let normalized = normalized_squared(&nominal, &innov);
let log_likelihood = gaussian_log_density::<D>(&nominal, normalized);
let Some(scale) = policy.variance_scale(normalized, D) else {
return (robust::rejected(normalized), log_likelihood);
};
#[allow(clippy::float_cmp)]
let undiscounted = scale == 1.0;
let sy = if undiscounted {
nominal
} else {
innovation_factor::<D, L>(&resid, &w, noise_sqrt, scale)
};
let mut pxy = [[0.0; D]; N];
for i in 1..L {
for a in 0..N {
let da = pts[i][a] - self.x[a];
for b in 0..D {
pxy[a][b] += w.wi * da * resid[i][b];
}
}
}
let k = solve_gain::<N, D>(&pxy, &sy);
for a in 0..N {
let mut acc = 0.0;
for b in 0..D {
acc += k[a][b] * innov[b];
}
self.x[a] += acc;
}
let mut u = [[0.0; D]; N];
for a in 0..N {
for j in 0..D {
let mut acc = 0.0;
for c in 0..D {
acc += k[a][c] * sy[c][j];
}
u[a][j] = acc;
}
}
for j in 0..D {
let mut col = [0.0; N];
for a in 0..N {
col[a] = u[a][j];
}
cholupdate(&mut self.s, &col, -1.0);
}
let mut trace = 0.0;
for i in 0..D {
for j in 0..=i {
trace += sy[i][j] * sy[i][j];
}
}
#[allow(clippy::cast_precision_loss)] let outcome = RobustOutcome {
accepted: true,
weight: 1.0 / scale,
nominal_normalized_squared: normalized,
innovation_variance: trace / D as f64,
};
(outcome, log_likelihood)
}
#[must_use]
pub fn covariance(&self) -> [[f64; N]; N] {
let mut p = [[0.0; N]; N];
for i in 0..N {
for j in 0..N {
let mut acc = 0.0;
for k in 0..N {
acc += self.s[i][k] * self.s[j][k];
}
p[i][j] = acc;
}
}
p
}
#[must_use]
pub fn covariance_sqrt(&self) -> [[f64; N]; N] {
self.s
}
}
#[must_use]
pub(crate) fn unscented_predict<const N: usize, const L: usize>(
params: &UnscentedParams,
mean: &[f64; N],
sqrt: &[[f64; N]; N],
dynamics: impl Fn(&[f64; N]) -> [f64; N],
process_noise_sqrt: &[[f64; N]; N],
) -> ([f64; N], [[f64; N]; N]) {
let w = params.weights::<N>();
let pts = sigma_points::<N, L>(mean, sqrt, w.gamma);
let mut prop = [[0.0; N]; L];
for i in 0..L {
prop[i] = dynamics(&pts[i]);
}
let (offset, resid) = centered_residuals::<N, L>(&prop, &w);
let mut xm = [0.0; N];
for j in 0..N {
xm[j] = prop[0][j] + offset[j];
}
let sqrt_wi = libm::sqrt(w.wi);
let mut r = [[0.0; N]; N];
for i in 1..L {
let mut row = [0.0; N];
for j in 0..N {
row[j] = sqrt_wi * resid[i][j];
}
fold_row(&mut r, &row);
}
for k in 0..N {
let mut col = [0.0; N];
for i in 0..N {
col[i] = process_noise_sqrt[i][k];
}
fold_row(&mut r, &col);
}
let mut s = transpose_to_lower(&r);
let mut center = [0.0; N];
let mag = libm::sqrt(libm::fabs(w.w0c));
for j in 0..N {
center[j] = mag * resid[0][j];
}
cholupdate(&mut s, ¢er, if w.w0c < 0.0 { -1.0 } else { 1.0 });
(xm, s)
}
fn gaussian_log_density<const D: usize>(factor: &[[f64; D]; D], normalized_squared: f64) -> f64 {
let mut log_det = 0.0;
for i in 0..D {
log_det += libm::log(libm::fabs(factor[i][i]));
}
#[allow(clippy::cast_precision_loss)] let dimension = D as f64;
-0.5 * (normalized_squared + dimension * libm::log(core::f64::consts::TAU)) - log_det
}
fn innovation_factor<const D: usize, const L: usize>(
resid: &[[f64; D]; L],
w: &crate::unscented::Weights,
noise_sqrt: &[[f64; D]; D],
scale: f64,
) -> [[f64; D]; D] {
let sqrt_wi = libm::sqrt(w.wi);
let root = libm::sqrt(scale);
let mut ry = [[0.0; D]; D];
for i in 1..L {
let mut row = [0.0; D];
for j in 0..D {
row[j] = sqrt_wi * resid[i][j];
}
fold_row(&mut ry, &row);
}
for k in 0..D {
let mut col = [0.0; D];
for i in 0..D {
col[i] = root * noise_sqrt[i][k];
}
fold_row(&mut ry, &col);
}
let mut sy = transpose_to_lower(&ry);
let mut ycen = [0.0; D];
let mag = libm::sqrt(libm::fabs(w.w0c));
for j in 0..D {
ycen[j] = mag * resid[0][j];
}
cholupdate(&mut sy, &ycen, if w.w0c < 0.0 { -1.0 } else { 1.0 });
sy
}
fn fold_row<const M: usize>(r: &mut [[f64; M]; M], row: &[f64; M]) {
let mut a = *row;
for k in 0..M {
if a[k] == 0.0 {
continue;
}
let rkk = r[k][k];
let denom = libm::hypot(rkk, a[k]);
if denom == 0.0 {
continue;
}
let c = rkk / denom;
let s = a[k] / denom;
r[k][k] = denom;
for j in (k + 1)..M {
let temp = c * r[k][j] + s * a[j];
a[j] = c * a[j] - s * r[k][j];
r[k][j] = temp;
}
}
}
fn transpose_to_lower<const M: usize>(r: &[[f64; M]; M]) -> [[f64; M]; M] {
let mut l = [[0.0; M]; M];
for i in 0..M {
for j in 0..M {
l[i][j] = r[j][i];
}
}
l
}
fn cholupdate<const M: usize>(l: &mut [[f64; M]; M], v: &[f64; M], sign: f64) {
let mut x = *v;
for k in 0..M {
let dk = l[k][k];
if dk == 0.0 {
continue;
}
let rr = dk * dk + sign * x[k] * x[k];
let r = if rr > 0.0 { libm::sqrt(rr) } else { 0.0 };
l[k][k] = r;
if r == 0.0 {
continue;
}
let c = r / dk;
let s = x[k] / dk;
for i in (k + 1)..M {
l[i][k] = (l[i][k] + sign * s * x[i]) / c;
x[i] = c * x[i] - s * l[i][k];
}
}
}
#[must_use]
fn solve_gain<const N: usize, const D: usize>(
pxy: &[[f64; D]; N],
sy: &[[f64; D]; D],
) -> [[f64; D]; N] {
let mut m = [[0.0; D]; N];
for n in 0..N {
for j in 0..D {
let mut acc = pxy[n][j];
for k in 0..j {
acc -= m[n][k] * sy[j][k];
}
m[n][j] = if sy[j][j] == 0.0 { 0.0 } else { acc / sy[j][j] };
}
}
let mut k = [[0.0; D]; N];
for n in 0..N {
for j in (0..D).rev() {
let mut acc = m[n][j];
for c in (j + 1)..D {
acc -= k[n][c] * sy[c][j];
}
k[n][j] = if sy[j][j] == 0.0 { 0.0 } else { acc / sy[j][j] };
}
}
k
}
#[cfg(test)]
mod tests {
#![allow(clippy::unwrap_used, clippy::float_cmp, clippy::cast_precision_loss)]
use super::*;
use crate::kalman::SquareRootKalman;
use crate::unscented::unscented_transform;
use spacelab_testkit::nan_max;
fn dot<const N: usize>(a: &[f64; N], b: &[f64; N]) -> f64 {
(0..N).map(|i| a[i] * b[i]).sum()
}
fn matvec<const N: usize>(a: &[[f64; N]; N], x: &[f64; N]) -> [f64; N] {
let mut y = [0.0; N];
for i in 0..N {
for j in 0..N {
y[i] += a[i][j] * x[j];
}
}
y
}
#[test]
fn the_srukf_reproduces_the_linear_kalman_filter() {
let (worst_x, worst_p) = linear_kalman_departure(UnscentedParams::new(1.0, 2.0, 0.0));
println!(
"SR-UKF vs SR-KF (linear-Gaussian): max |Δx| {worst_x:.3e}, max |ΔP| {worst_p:.3e}"
);
assert!(worst_x < 1e-11, "state drifted from the KF: {worst_x:.3e}");
assert!(
worst_p < 1e-11,
"covariance drifted from the KF: {worst_p:.3e}"
);
}
#[test]
fn the_srukf_reproduces_the_linear_kalman_filter_through_the_downdate() {
let params = UnscentedParams::gaussian();
let w0c = params.weights::<3>().w0c;
assert!(
w0c < 0.0,
"the fixture must reach the downdate, W₀ᶜ = {w0c}"
);
let (worst_x, worst_p) = linear_kalman_departure(params);
println!("SR-UKF vs SR-KF (α = 1e-3): max |Δx| {worst_x:.3e}, max |ΔP| {worst_p:.3e}");
assert!(worst_x < 5e-9, "state drifted from the KF: {worst_x:.3e}");
assert!(
worst_p < 1e-12,
"covariance drifted from the KF: {worst_p:.3e}"
);
}
#[test]
fn the_posterior_survives_a_state_that_dwarfs_its_uncertainty() {
let (p0, sigma) = ([[1e-6, 0.0], [0.0, 4e-6]], 1e-3);
let x0 = [1e6, 1e6];
let mut filter = SquareRootUnscentedKalman::<2, 5>::from_covariance(
x0,
&p0,
UnscentedParams::gaussian(),
)
.unwrap();
let predicted = x0[0] + x0[1];
let z = predicted + 1e-3;
let innovation = z - predicted;
filter.update(|x| [x[0] + x[1]], &[z], &[[sigma]]);
let hph_r = p0[0][0] + p0[1][1] + sigma * sigma;
let gain = [p0[0][0] / hph_r, p0[1][1] / hph_r];
let posterior = filter.covariance();
for a in 0..2 {
let want = gain[a] * innovation;
let error = ((filter.x[a] - x0[a]) - want).abs() / want.abs();
assert!(error < 1e-5, "correction[{a}]: relative error {error:e}");
for b in 0..2 {
let want = if a == b { p0[a][a] } else { 0.0 } - gain[a] * hph_r * gain[b];
let error = (posterior[a][b] - want).abs() / want.abs();
assert!(error < 1e-5, "P⁺[{a}][{b}]: relative error {error:e}");
}
}
}
#[test]
fn the_reported_factor_multiplies_out_to_the_reported_covariance() {
let s0 = [[2.0, 0.0, 0.0], [0.4, 1.7, 0.0], [-0.2, 0.3, 1.3]];
let mut ukf = SquareRootUnscentedKalman::<3, 7>::new(
[0.5, -0.3, 0.1],
s0,
UnscentedParams::gaussian(),
);
ukf.update::<1>(|x| [x[0] + 0.5 * x[2]], &[1.25], &[[0.5]]);
let (s, p) = (ukf.covariance_sqrt(), ukf.covariance());
for i in 0..3 {
for j in 0..3 {
if j > i {
assert_eq!(s[i][j], 0.0, "S[{i}][{j}] is above the diagonal");
}
let sst: f64 = (0..3).map(|k| s[i][k] * s[j][k]).sum();
assert!(
(sst - p[i][j]).abs() < 1e-12,
"S Sᵀ[{i}][{j}] = {sst}, covariance says {}",
p[i][j]
);
}
}
assert!(s[0][0] > 0.0, "a covariance factor cannot be all zeros");
}
fn linear_kalman_departure(params: UnscentedParams) -> (f64, f64) {
let phi = [[1.0, 0.05, 0.0], [0.0, 1.0, 0.05], [0.02, 0.0, 1.0]];
let sq = [[0.10, 0.0, 0.0], [0.02, 0.12, 0.0], [0.0, 0.03, 0.15]];
let s0 = [[2.0, 0.0, 0.0], [0.4, 1.7, 0.0], [-0.2, 0.3, 1.3]];
let x0 = [0.5, -0.3, 0.1];
let mut ukf = SquareRootUnscentedKalman::<3, 7>::new(x0, s0, params);
let mut kf = SquareRootKalman::<3>::new(x0, s0);
let rows = [
[1.0, 0.0, 0.0],
[0.0, 1.0, 0.0],
[0.0, 0.0, 1.0],
[1.0, 0.5, 0.2],
];
let r = 0.25_f64;
let mut truth = [1.0, 2.0, -1.0];
let mut worst_x = 0.0_f64;
let mut worst_p = 0.0_f64;
for step in 0..12 {
truth = matvec(&phi, &truth);
ukf.predict(|x| matvec(&phi, x), &sq);
kf.time_update(&phi, &sq);
kf.x = matvec(&phi, &kf.x);
let h = rows[step % 4];
let measured = dot(&h, &truth);
ukf.update::<1>(|x| [dot(&h, x)], &[measured], &[[libm::sqrt(r)]]);
kf.measurement_update(&h, measured - dot(&h, &kf.x), r);
let pu = ukf.covariance();
let pk = kf.covariance();
for i in 0..3 {
worst_x = nan_max(worst_x, (ukf.x[i] - kf.x[i]).abs());
for j in 0..3 {
worst_p = nan_max(worst_p, (pu[i][j] - pk[i][j]).abs());
}
}
}
(worst_x, worst_p)
}
#[test]
fn the_srukf_vector_update_matches_sequential_scalar() {
let h = [[1.0, 0.5, 0.2], [-0.3, 1.0, 0.4]];
let rvar = [0.20_f64, 0.35_f64];
let s0 = [[1.8, 0.0, 0.0], [0.5, 1.4, 0.0], [-0.2, 0.3, 1.1]];
let x0 = [0.4, -0.6, 0.2];
let params = UnscentedParams::new(1.0, 2.0, 0.0);
let mut ukf = SquareRootUnscentedKalman::<3, 7>::new(x0, s0, params);
let mut kf = SquareRootKalman::<3>::new(x0, s0);
let z = [1.1, -0.4];
ukf.update::<2>(
|x| [dot(&h[0], x), dot(&h[1], x)],
&z,
&[[libm::sqrt(rvar[0]), 0.0], [0.0, libm::sqrt(rvar[1])]],
);
for r in 0..2 {
kf.measurement_update(&h[r], z[r] - dot(&h[r], &kf.x), rvar[r]);
}
let pu = ukf.covariance();
let pk = kf.covariance();
let mut worst = 0.0_f64;
for i in 0..3 {
worst = nan_max(worst, (ukf.x[i] - kf.x[i]).abs());
for j in 0..3 {
worst = nan_max(worst, (pu[i][j] - pk[i][j]).abs());
}
}
println!("SR-UKF vector (D=2) vs sequential scalar KF: max |Δx,ΔP| {worst:.3e}");
assert!(
worst < 1e-11,
"vector update disagrees with sequential scalar: {worst:.3e}"
);
}
fn reference_update<const N: usize, const L: usize>(
params: &UnscentedParams,
x: &mut [f64; N],
p: &mut [[f64; N]; N],
measurement: impl Fn(&[f64; N]) -> [f64; 1],
z: f64,
r: f64,
) {
let s = cholesky::<N>(p).unwrap();
let (yb, pyy0) = unscented_transform::<N, 1, L>(params, x, &s, &measurement);
let pyy = pyy0[0][0] + r;
let w = params.weights::<N>();
let pts = sigma_points::<N, L>(x, &s, w.gamma);
let mut pxy = [0.0; N];
for i in 1..L {
let dy = measurement(&pts[i])[0] - yb[0];
for a in 0..N {
pxy[a] += w.wi * (pts[i][a] - x[a]) * dy;
}
}
let mut k = [0.0; N];
for a in 0..N {
k[a] = pxy[a] / pyy;
}
let innov = z - yb[0];
for a in 0..N {
x[a] += k[a] * innov;
}
for a in 0..N {
for b in 0..N {
p[a][b] -= k[a] * k[b] * pyy;
}
}
}
#[test]
fn the_square_root_form_equals_the_plain_ukf() {
let worst = square_root_vs_covariance(UnscentedParams::new(0.7, 2.0, 0.0));
assert!(
worst < 1e-10,
"square-root and covariance forms disagree: {worst:.3e}"
);
}
#[test]
fn the_negative_centre_weight_subtracts_rather_than_adds() {
let params = UnscentedParams::new(0.9, 0.0, 0.0);
let w0c = params.weights::<2>().w0c;
assert!(
w0c < 0.0,
"the fixture must reach the downdate, W₀ᶜ = {w0c}"
);
let worst = square_root_vs_covariance(params);
assert!(
worst < 1e-10,
"square-root and covariance forms disagree at W₀ᶜ = {w0c}: {worst:.3e}"
);
}
fn square_root_vs_covariance(params: UnscentedParams) -> f64 {
let dt = 0.1;
let dynamics = |x: &[f64; 2]| {
[
x[0] + dt * x[1],
x[1] - dt * (libm::sin(x[0]) + 0.05 * x[1]),
]
};
let meas = |x: &[f64; 2]| [libm::sqrt(x[0] * x[0] + x[1] * x[1])];
let p0 = [[0.20, 0.02], [0.02, 0.10]];
let sq = [[0.03, 0.0], [0.0, 0.03]];
let x0 = [0.8, -0.2];
let r = 0.04_f64;
let mut ukf = SquareRootUnscentedKalman::<2, 5>::from_covariance(x0, &p0, params).unwrap();
let mut xref = x0;
let mut pref = p0;
let mut worst = 0.0_f64;
let mut truth = [1.0, 0.3];
for _ in 0..15 {
truth = dynamics(&truth);
ukf.predict(dynamics, &sq);
let sref = cholesky::<2>(&pref).unwrap();
let (xm, mut cov) = unscented_transform::<2, 2, 5>(¶ms, &xref, &sref, dynamics);
for i in 0..2 {
for j in 0..2 {
cov[i][j] += sq[i][0] * sq[j][0] + sq[i][1] * sq[j][1];
}
}
xref = xm;
pref = cov;
let measured = meas(&truth)[0];
ukf.update::<1>(meas, &[measured], &[[libm::sqrt(r)]]);
reference_update::<2, 5>(¶ms, &mut xref, &mut pref, meas, measured, r);
let pu = ukf.covariance();
for i in 0..2 {
worst = nan_max(worst, (ukf.x[i] - xref[i]).abs());
for j in 0..2 {
worst = nan_max(worst, (pu[i][j] - pref[i][j]).abs());
}
}
}
println!("SR-UKF vs covariance-form UKF (nonlinear): max |ΔP,Δx| {worst:.3e}");
worst
}
#[test]
fn the_covariance_stays_positive_definite() {
let params = UnscentedParams::new(0.9, 2.0, 0.0);
let dt = 0.05;
let dynamics = |x: &[f64; 2]| [x[0] + dt * x[1], x[1] - dt * libm::sin(x[0])];
let meas = |x: &[f64; 2]| [x[0]];
let sq = [[0.02, 0.0], [0.0, 0.02]];
let mut ukf = SquareRootUnscentedKalman::<2, 5>::from_covariance(
[0.5, 0.0],
&[[0.3, 0.0], [0.0, 0.2]],
params,
)
.unwrap();
for _ in 0..40 {
ukf.predict(dynamics, &sq);
ukf.update::<1>(meas, &[0.4], &[[0.1]]);
let p = ukf.covariance();
for i in 0..2 {
for j in 0..2 {
assert!((p[i][j] - p[j][i]).abs() < 1e-12, "asymmetry");
}
}
assert!(cholesky::<2>(&p).is_some(), "lost positive-definiteness");
}
}
#[test]
fn the_outcome_reports_the_variance_the_discount_actually_admitted() {
let params = UnscentedParams::gaussian();
let x0 = [0.4, -0.2];
let p0 = [[4e-2, 1e-2], [1e-2, 2.5e-2]];
let sigma = 0.3;
let noise_sqrt = [[sigma, 0.0], [0.0, sigma]];
let z = [1.1, -0.9];
let measurement = |x: &[f64; 2]| [x[0], x[0] + x[1]];
let mut plain =
SquareRootUnscentedKalman::<2, 5>::from_covariance(x0, &p0, params).unwrap();
let base = plain.update_robust(measurement, &z, &noise_sqrt, RobustPolicy::Plain);
assert!(base.accepted);
assert_eq!(base.weight, 1.0);
let policy = RobustPolicy::ChiSquareInflation {
threshold: base.nominal_normalized_squared / 2.0,
};
let scale = policy
.variance_scale(base.nominal_normalized_squared, 2)
.unwrap();
assert!(scale > 1.5, "the control must actually discount: {scale}");
let mut inflated =
SquareRootUnscentedKalman::<2, 5>::from_covariance(x0, &p0, params).unwrap();
let out = inflated.update_robust(measurement, &z, &noise_sqrt, policy);
assert_eq!(
out.nominal_normalized_squared,
base.nominal_normalized_squared
);
assert_eq!(out.weight, 1.0 / scale);
let admitted = out.innovation_variance - base.innovation_variance;
let expected = sigma * sigma * (scale - 1.0);
assert!(
(admitted - expected).abs() < 1e-9 * expected,
"admitted {admitted:.17e}, expected {expected:.17e}"
);
}
#[test]
fn the_log_density_carries_the_normalizing_constant_of_a_gaussian() {
let factor = [[2.0, 0.0], [1.0, 3.0]];
let normalized_squared = 1.5;
let log_density = gaussian_log_density(&factor, normalized_squared);
let determinant = (factor[0][0] * factor[1][1]).powi(2);
let expected = (-0.5 * normalized_squared).exp()
/ (core::f64::consts::TAU.powi(2) * determinant).sqrt();
let density = log_density.exp();
assert!(
(density - expected).abs() < 1e-15 * expected,
"density {density:.17e}, expected {expected:.17e}"
);
}
}