///|
fn observation_matrix() -> @kalman.Matrix {
@kalman.Matrix::from_rows([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0]])
}
///|
fn measurement_matrix(covariance : @types.Covariance2) -> @kalman.Matrix {
@kalman.Matrix::from_rows([
[covariance.xx, covariance.xy],
[covariance.xy, covariance.yy],
])
}
///|
/// The upstream convenience helper models piecewise-constant acceleration.
/// This adapter instead uses continuous white acceleration with intensity q.
fn process_matrix(dt : Double, q : Double) -> @kalman.Matrix {
let pp = q * dt * dt * dt / 3.0
let pv = q * dt * dt / 2.0
let vv = q * dt
@kalman.Matrix::from_rows([
[pp, 0.0, pv, 0.0],
[0.0, pp, 0.0, pv],
[pv, 0.0, vv, 0.0],
[0.0, pv, 0.0, vv],
])
}
///|
fn healthy_state(state : Array[Double], covariance : @kalman.Matrix) -> Bool {
if state.length() != 4 ||
covariance.rows() != 4 ||
covariance.cols() != 4 ||
!covariance.is_finite() {
return false
}
for value in state {
if !@types.is_finite(value) {
return false
}
}
// Scale before the upstream eigenvalue diagnostic to avoid fixed-unit
// tolerances and overflow in extreme but finite public input.
let scale = covariance.max_abs()
if !@types.is_finite(scale) || scale <= 0.0 {
return false
}
let normalized = covariance.map(value => value / scale)
normalized.is_finite() &&
@kalman.covariance_is_psd(normalized, 0.000000001) &&
covariance_consistent(covariance)
}
///|
/// Check each pair in its own diagonal scale: a large velocity prior must not
/// hide asymmetric or indefinite position/cross covariance. Normalize only a
/// diagnostic copy, leaving the filter's original covariance entries untouched.
fn covariance_consistent(covariance : @kalman.Matrix) -> Bool {
let normalized = @kalman.Matrix::identity(4)
for row in 0..<4 {
if covariance.get(row, row) <= 0.0 {
return false
}
for col in (row + 1)..<4 {
let scale = covariance.get(row, row).sqrt() *
covariance.get(col, col).sqrt()
if !@types.is_finite(scale) || scale <= 0.0 {
return false
}
let upper = covariance.get(row, col) / scale
let lower = covariance.get(col, row) / scale
if !@types.is_finite(upper) ||
!@types.is_finite(lower) ||
upper.abs() > 1.000000001 ||
lower.abs() > 1.000000001 ||
(upper - lower).abs() > 0.000000001 {
return false
}
ignore(normalized.set(row, col, upper))
ignore(normalized.set(col, row, lower))
}
}
@kalman.covariance_is_psd(normalized, 0.000000001)
}
///|
fn covariance_block(
covariance : @kalman.Matrix,
offset : Int32,
) -> @types.Covariance2? {
let result : @types.Covariance2 = {
xx: covariance.get(offset, offset),
xy: covariance.get(offset, offset + 1),
yy: covariance.get(offset + 1, offset + 1),
}
try {
result.validate()
Some(result)
} catch {
_ => None
}
}
///|
/// sqrt(lambda_max) without forming an overflowing eigenvalue or squaring an
/// uncertainty limit. Normalized terms stay within a small bounded range.
fn maximum_direction_std(covariance : @types.Covariance2) -> Double {
let scale = if covariance.xx > covariance.yy {
covariance.xx
} else {
covariance.yy
}
let xx = covariance.xx / scale
let yy = covariance.yy / scale
let xy = covariance.xy / scale
let eigenvalue = (xx + yy) / 2.0 + norm2((xx - yy) / 2.0, xy)
scale.sqrt() * eigenvalue.sqrt()
}
///|
fn covariance_std(covariance : @kalman.Matrix, offset : Int32) -> Double? {
covariance_block(covariance, offset).map(maximum_direction_std)
}
///|
fn flatten_covariance(covariance : @kalman.Matrix) -> Array[Double] {
Array::makei(16, i => covariance.get(i / 4, i % 4))
}
///|
fn norm2(x : Double, y : Double) -> Double {
let scale = if x.abs() > y.abs() { x.abs() } else { y.abs() }
if scale == 0.0 {
0.0
} else {
let nx = x / scale
let ny = y / scale
scale * (nx * nx + ny * ny).sqrt()
}
}