LeastSquaresFilter

Copyright 2015 Roger R Labbe Jr.

FilterPy library. http://github.com/rlabbe/filterpy

Documentation at: https://filterpy.readthedocs.org

Supporting book at: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python

This is licensed under an MIT license. See the readme.MD file for more information.

class filterpy.leastsq.LeastSquaresFilter(dt, order, noise_sigma=0.0)[source]

Implements a Least Squares recursive filter. Formulation is per Zarchan [R13].

Filter may be of order 0 to 2. Order 0 assumes the value being tracked is a constant, order 1 assumes that it moves in a line, and order 2 assumes that it is tracking a second order polynomial.

Parameters

dt : float

time step per update

order : int

order of filter 0..2

noise_sigma : float

sigma (std dev) in x. This allows us to calculate the error of the filter, it does not influence the filter output.

References

R13(1,2)

Zarchan and Musoff. “Fundamentals of Kalman Filtering: A Practical Approach.” Third Edition. AIAA, 2009.

Examples

from filterpy.leastsq import LeastSquaresFilter

lsq = LeastSquaresFilter(dt=0.1, order=1, noise_sigma=2.3)

while True:
    z = sensor_reading()  # get a measurement
    x = lsq.update(z)     # get the filtered estimate.
    print('error: {}, velocity error: {}'.format(
          lsq.error, lsq.derror))

Attributes

n

(int) step in the recursion. 0 prior to first call, 1 after the first call, etc.

K

(np.array) Gains for the filter. K[0] for all orders, K[1] for orders 0 and 1, and K[2] for order 2

x: np.array (order + 1, 1)

estimate(s) of the output. It is a vector containing the estimate x and the derivatives of x: [x x’ x’’].T. It contains as many derivatives as the order allows. That is, a zero order filter has no derivatives, a first order has one derivative, and a second order has two.

y

(float) residual (difference between measurement projection of previous estimate to current time).

__init__(dt, order, noise_sigma=0.0)[source]

Initialize self. See help(type(self)) for accurate signature.

reset()[source]

reset filter back to state at time of construction

update(z)[source]

Update filter with new measurement z

Returns

x : np.array

estimate for this time step (same as self.x)

errors()[source]

Computes and returns the error and standard deviation of the filter at this time step.

Returns

error : np.array size 1xorder+1

std : np.array size 1xorder+1