NextControl/dev.nextftc.control.filters/KalmanFilter

KalmanFilter

class KalmanFilter<States : Nat, Inputs : Nat, Outputs : Nat> @JvmOverloads constructor(val plant: LinearModel<States, Inputs, Outputs>, val stateStdDevs: Vector<States>, val measurementStdDevs: Vector<Outputs>, dt: Double = 0.05)(source)

A Kalman filter for linear state estimation.

The Kalman filter is an optimal state estimator for linear systems with Gaussian noise. It combines predictions from a system model with noisy measurements to produce an optimal estimate of the system state.

The filter operates on a linear system of the form:

x(k+1) = A * x(k) + B * u(k) + w(k)    (state equation)
y(k)   = C * x(k) + D * u(k) + v(k)    (measurement equation)

where:

x is the state vector
u is the input vector
y is the measurement vector
w is process noise with covariance Q
v is measurement noise with covariance R

Parameters

plant

The linear system model describing the plant dynamics

stateStdDevs

Standard deviations of the state model (process noise)

measurementStdDevs

Standard deviations of the measurements (measurement noise)

The discretization timestep in seconds (default 0.05s / 50ms)

Type Parameters

States

The number of states in the system

Inputs

The number of inputs to the system

Outputs

The number of outputs (measurements) from the system

Constructors

KalmanFilter

@JvmOverloads

constructor(plant: LinearModel<States, Inputs, Outputs>, stateStdDevs: Vector<States>, measurementStdDevs: Vector<Outputs>, dt: Double = 0.05)

Properties

measurementStdDevs

val measurementStdDevs: Vector<Outputs>

plant

val plant: LinearModel<States, Inputs, Outputs>

stateStdDevs

val stateStdDevs: Vector<States>

Functions

correct

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

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

predict

fun predict(inputs: Vector<Inputs>): Vector<States>

Performs the prediction step of the Kalman filter.

reset

fun reset()

Resets the filter to its initial state.