Innovation function and other functions as inputs

[matthewgcooper]

Hi, thanks for the great package. In the past I used FilterPy to create UKF's for vehicle state estimation. One important feature of FilterPy that I used was the ability to define an innovation function (see FilterPy UKF and the keyword argument residual_z).

Defining your own innovation function is important in vehicle state estimation where measurements can be angles. For example, a magnetometer can measure the heading of the vehicle, so the innovation needs to be wrapped (if using z = y -y_pred with y = 1deg and y_pred = 359deg, then the innovation will be very large even though it should be small).

Would you consider allowing the user to input in their own custom functions that override the internal functions in predict! and update!? Other functions like the mean and covariance calculations would be useful to when the state includes a quaternion (see this paper, Section 3.4 on why a different mean function is required).

[baggepinnen]

Hello Matthew, yeah this is a good idea and should be quick to add. It's also generally useful when you perform filtering with DAE systems, where you may not be able to compute arithmetic means of points on some nonlinear manifold.

[baggepinnen]

Check out c5ee9c8 which I accidentally pushed as a commit to master instead of a PR branch :/

It adds support for custom mean and innovation functions. A custom covariance requires slightly more care since there are two different types of covariance computed in an UKF, the covariance of the dynamcis sigma points and the cross covariance between dynamics and output sigma points. You can pass a custom covariance function already in the commit above, but I did not yet document this feature since the custom covariance is only used for the dynamics points.

---

Edit:

This PR adds support for custom covariance functions as well.

• add option to supply custom covariance functions to UKF #169

Please check it out and see if it satisfies your needs before I make a new release including this functionality :)

[matthewgcooper]

Thanks heaps! I'll check this out today using my heading measurement function.

[matthewgcooper]

Okay so I tested and the innovation function works well. The mean and covar functions work for the prediction step. However, in the measurement step I believe we'll need the ability to define the mean and covar functions there as well (as using the standard mean and covar functions for a set of angle sigma points is not correct).

Another option could be to allow the user to input two residual functions (one for the state and one for the measurement) instead of a covar function (under the assumption that the covariance is computed using the residuals of the sigma points to the mean). In this case, a residual function would be used to compute the predicted state covariance and another residual function would be used to compute both the innovation and the innovation covariance.

I'm not too sure how you've currently implemented the standard covariance function so I'm not sure if using a residual function inside it will work. This is how FilterPy implements it if you're interested:

    # new covariance is the sum of the outer product of the residuals
    # times the weights

    # this is the fast way to do this - see 'else' for the slow way
    if residual_fn is np.subtract or residual_fn is None:
        y = sigmas - x[np.newaxis, :]
        P = np.dot(y.T, np.dot(np.diag(Wc), y))
    else:
        P = np.zeros((n, n))
        for k in range(kmax):
            y = residual_fn(sigmas[k], x)
            P += Wc[k] * np.outer(y, y)

and from there documentation:

x_mean_fn : callable (sigma_points, weights), optional

Function that computes the mean of the provided sigma points and weights. Use this if your state variable contains nonlinear values such as angles which cannot be summed.

def state_mean(sigmas, Wm):
    x = np.zeros(3)
    sum_sin, sum_cos = 0., 0.

    for i in range(len(sigmas)):
        s = sigmas[i]
        x[0] += s[0] * Wm[i]
        x[1] += s[1] * Wm[i]
        sum_sin += sin(s[2])*Wm[i]
        sum_cos += cos(s[2])*Wm[i]
    x[2] = atan2(sum_sin, sum_cos)
    return x

z_mean_fn : callable (sigma_points, weights), optional

Same as x_mean_fn, except it is called for sigma points which form the measurements after being passed through hx().

residual_x : callable (x, y), optional

residual_z : callable (x, y), optional

Function that computes the residual (difference) between x and y. You will have to supply this if your state variable cannot support subtraction, such as angles (359-1 degreees is 2, not 358). x and y are state vectors, not scalars. One is for the state variable, the other is for the measurement state.

def residual(a, b):
    y = a[0] - b[0]
    if y > np.pi:
        y -= 2*np.pi
    if y < -np.pi:
        y = 2*np.pi
    return y

[baggepinnen]

I added separate functions for predict! and correct! in the PR above. I believe being able to override the covariance functions (rather than just the innovation function) gives maximum flexibility, potentially useful for some geometries where inner products aren't computed as the standard Euclidean dot product.

[matthewgcooper]

okay thanks for doing that. I'll be testing it with my code today

[baggepinnen]

I decided to redesign the internals of how the measurement update is handled, in

• allow multiple measurement models in Kalman filters #171

I introduce the concept of a measurement model, which allows all Kalman-type filters to have one or several measurement models. This is useful to allow

• Performing measurement updates with different sensors at different rates
• Using a measurement model from a different kind of filter, e.g., performing the measurement update from a standard kalman filter with a dynamics update from an UKF/EKF

[baggepinnen]

I hope the new release addresses your issue, if not, feel free to continue the discussion here