feat: add ExtendedKalmanFilter, the Kalman filter for a nonlinear model (#7615)

The ordinary Kalman filter is optimal only for a linear model, and most things worth tracking are not linear: a bearing is an arctangent of the state, a range a square root of it. The extended filter pushes the state forward through the real functions and the covariance through their Jacobians, evaluated afresh at the current estimate, which keeps the whole machinery and replaces only the step that needed linearity.

The covariance correction is written in Joseph form rather than as the shorter (I - K H) P. The two agree in exact arithmetic, but the short one subtracts nearly equal matrices and can leave a covariance that is no longer symmetric or has a negative variance on its diagonal; the Joseph form is a sum of two products of the form A P A' and stays symmetric by construction. The innovation covariance is inverted with the existing matrix.InverseOfMatrix instead of another copy of Gaussian elimination.

The Javadoc states what the linearisation costs, namely that the filter is no longer optimal and can diverge on a sharp nonlinearity without the covariance warning about it. Tests: one update of a scalar filter is the textbook half way step, a linear model reproduces the scalar Kalman recursion to 1e-12 over 500 steps, the covariance stays symmetric over 500 steps of a three state model, and a target seen only as a range is tracked through the nonlinear measurement more accurately than inverting each reading on its own.

Signed-off-by: alxkm <19151554+alxkm@users.noreply.github.com>
Co-authored-by: alxkm <19151554+alxkm@users.noreply.github.com>

Author: alxkm