Class ExtendedKalman

Class Documentation

class ExtendedKalman

The kalman filter can be used by instantiating an ExtendedKalman object and calling filterData.

Example source:

float sensorData;
float filtered;
ExtendedKalman kalman(1.0f, 0.0f);

while(1)
{
    filtered = kalman.filterData(sensorData);
}

Public Functions

ExtendedKalman(float tQ, float tR)

Initializes a kalman filter with the given covariances.

Note

tR can be fixed at 1 and then tQ can be modified (since the magnitude of tQ and tR are relative to each other). Larger tQ means more trust in the data we are measuring. Conversely, a smaller tQ means more trust in the model’s prediction (rather than the measured) value.

Parameters:

  • tQ – [in] the system noise covariance.

  • tR – [in] the measurement noise covariance.

float filterData(float dat)

Runs the kalman filter, returning the current prediction.

Note

description of data:\(x(k | k)\)

is the current prediction (filtered output)

(and then

\(k - 1\)

would be the previous output)

Corresponding formula:

\[\begin{split}\begin{eqnarray*} x(k | k-1) & = & A \cdot X(k-1 | k-1) + B \cdot U(k) + W(K)\\ p(k | k-1) & = & A \cdot p(k-1 | k-1) \cdot A^\prime + Q\\ kg(k) & = & p(k | k-1) \cdot \frac{H^\prime}{H \cdot p(k | k-1) \cdot H^\prime + R}\\ x(k | k) & = & X(k | k - 1) + kg(k) \cdot (Z(k) - H \cdot X(k | k-1))\\ p(k | k) & = & (I - kg(k) \cdot H) \cdot P(k | k-1) \end{eqnarray*}\end{split}\]

Parameters:

dat – [in] the value to be filtered.

Returns:

The current prediction of what the data should be.

float getLastFiltered() const

Returns the last filtered data point.

void reset()

Resets the covariances and predictions.