NextControl/dev.nextftc.control.filters/KalmanFilter/correct

correct

fun correct(inputs: Vector<Inputs>, outputs: Vector<Outputs>): Vector<States>(source)

Performs the correction (update) step of the Kalman filter.

This incorporates a new measurement to refine the state estimate using the Joseph form of the covariance update for numerical stability:

S = C * P * Cᵀ + R                           (innovation covariance)
K = P * Cᵀ * S⁻¹                             (Kalman gain)
x̂(k|k) = x̂(k|k-1) + K * (y - D * u)         (state update)
P(k|k) = (I - K*C) * P * (I - K*C)ᵀ + K*R*Kᵀ (covariance update)

Return

The corrected state estimate x̂(k|k)

Parameters

inputs

The current input vector u(k)

outputs

The measurement vector y(k)