Skip to content

Square Root Filter

SquareRootKalmanFilter

Introduction and Overview

This implements a square root Kalman filter. No real attempt has been made to make this fast; it is a pedalogical exercise. The idea is that by computing and storing the square root of the covariance matrix we get about double the significant number of bits. Some authors consider this somewhat unnecessary with modern hardware. Of course, with microcontrollers being all the rage these days, that calculus has changed. But, will you really run a Kalman filter in Python on a tiny chip? No. So, this is for learning.

API Reference

SquareRootKalmanFilter

Bases: object

Create a Kalman filter which uses a square root implementation. This uses the square root of the state covariance matrix, which doubles the numerical precision of the filter, Therebuy reducing the effect of round off errors.

It is likely that you do not need to use this algorithm; we understand divergence issues very well now. However, if you expect the covariance matrix P to vary by 20 or more orders of magnitude then perhaps this will be useful to you, as the square root will vary by 10 orders of magnitude. From my point of view this is merely a 'reference' algorithm; I have not used this code in real world software. Brown[1] has a useful discussion of when you might need to use the square root form of this algorithm.

You are responsible for setting the various state variables to reasonable values; the defaults below will not give you a functional filter.

Parameters:

Name Type Description Default
dim_x int

Number of state variables for the Kalman filter. For example, if you are tracking the position and velocity of an object in two dimensions, dim_x would be 4.

This is used to set the default size of P, Q, and u

 required
dim_z int

Number of of measurement inputs. For example, if the sensor provides you with position in (x,y), dim_z would be 2.

 required
dim_u int(optional)

size of the control input, if it is being used. Default value of 0 indicates it is not used.

 0

Attributes:

Name Type Description
x array(dim_x, 1)

State estimate
P array(dim_x, dim_x)

State covariance matrix
x_prior array(dim_x, 1)

Prior (predicted) state estimate. The _prior and _post attributes are for convienence; they store the prior and posterior of the current epoch. Read Only.
P_prior array(dim_x, dim_x)

Prior (predicted) state covariance matrix. Read Only.
x_post array(dim_x, 1)

Posterior (updated) state estimate. Read Only.
P_post array(dim_x, dim_x)

Posterior (updated) state covariance matrix. Read Only.
z array

Last measurement used in update(). Read only.
R array(dim_z, dim_z)

Measurement noise matrix
Q array(dim_x, dim_x)

Process noise matrix
F array()

State Transition matrix
H array(dim_z, dim_x)

Measurement function
y array

Residual of the update step. Read only.
K array(dim_x, dim_z)

Kalman gain of the update step. Read only.

Examples:

See my book Kalman and Bayesian Filters in Python https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python

References

[1] Robert Grover Brown. Introduction to Random Signals and Applied Kalman Filtering. Wiley and sons, 2012.

Source code in bayesian_filters/kalman/square_root.py 
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
class SquareRootKalmanFilter(object):
    """

    Create a Kalman filter which uses a square root implementation.
    This uses the square root of the state covariance matrix, which doubles
    the numerical precision of the filter, Therebuy reducing the effect
    of round off errors.

    It is likely that you do not need to use this algorithm; we understand
    divergence issues very well now. However, if you expect the covariance
    matrix P to vary by 20 or more orders of magnitude then perhaps this
    will be useful to you, as the square root will vary by 10 orders
    of magnitude. From my point of view this is merely a 'reference'
    algorithm; I have not used this code in real world software. Brown[1]
    has a useful discussion of when you might need to use the square
    root form of this algorithm.

    You are responsible for setting the various state variables to
    reasonable values; the defaults below will not give you a functional
    filter.

    Parameters
    ----------

    dim_x : int
        Number of state variables for the Kalman filter. For example, if
        you are tracking the position and velocity of an object in two
        dimensions, dim_x would be 4.

        This is used to set the default size of P, Q, and u

    dim_z : int
        Number of of measurement inputs. For example, if the sensor
        provides you with position in (x,y), dim_z would be 2.

    dim_u : int (optional)
        size of the control input, if it is being used.
        Default value of 0 indicates it is not used.


    Attributes
    ----------

    x : numpy.array(dim_x, 1)
        State estimate

    P : numpy.array(dim_x, dim_x)
        State covariance matrix

    x_prior : numpy.array(dim_x, 1)
        Prior (predicted) state estimate. The *_prior and *_post attributes
        are for convienence; they store the  prior and posterior of the
        current epoch. Read Only.

    P_prior : numpy.array(dim_x, dim_x)
        Prior (predicted) state covariance matrix. Read Only.

    x_post : numpy.array(dim_x, 1)
        Posterior (updated) state estimate. Read Only.

    P_post : numpy.array(dim_x, dim_x)
        Posterior (updated) state covariance matrix. Read Only.

    z : numpy.array
        Last measurement used in update(). Read only.

    R : numpy.array(dim_z, dim_z)
        Measurement noise matrix

    Q : numpy.array(dim_x, dim_x)
        Process noise matrix

    F : numpy.array()
        State Transition matrix

    H : numpy.array(dim_z, dim_x)
        Measurement function

    y : numpy.array
        Residual of the update step. Read only.

    K : numpy.array(dim_x, dim_z)
        Kalman gain of the update step. Read only.

    Examples
    --------

    See my book Kalman and Bayesian Filters in Python
    https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python

    References
    ----------

    [1] Robert Grover Brown. Introduction to Random Signals and Applied
        Kalman Filtering. Wiley and sons, 2012.

    """

    def __init__(self, dim_x, dim_z, dim_u=0):
        if dim_z < 1:
            raise ValueError("dim_x must be 1 or greater")
        if dim_z < 1:
            raise ValueError("dim_x must be 1 or greater")
        if dim_u < 0:
            raise ValueError("dim_x must be 0 or greater")

        self.dim_x = dim_x
        self.dim_z = dim_z
        self.dim_u = dim_u

        self.x = zeros((dim_x, 1))  # state
        self._P = eye(dim_x)  # uncertainty covariance
        self._P1_2 = eye(dim_x)  # sqrt uncertainty covariance
        self._Q = eye(dim_x)  # sqrt process uncertainty
        self._Q1_2 = eye(dim_x)  # sqrt process uncertainty
        self.B = 0.0  # control transition matrix
        self.F = np.eye(dim_x)  # state transition matrix
        self.H = np.zeros((dim_z, dim_x))  # Measurement function
        self._R1_2 = eye(dim_z)  # sqrt state uncertainty
        self._R = eye(dim_z)  # state uncertainty
        self.z = np.array([[None] * self.dim_z]).T

        self.K = np.zeros((dim_x, dim_z))  # kalman gain
        self.S1_2 = np.zeros((dim_z, dim_z))  # sqrt system uncertainty
        self.SI1_2 = np.zeros((dim_z, dim_z))  # Inverse sqrt system uncertainty

        # Residual is computed during the innovation (update) step. We
        # save it so that in case you want to inspect it for various
        # purposes
        self.y = zeros((dim_z, 1))

        # identity matrix.
        self._I = np.eye(dim_x)

        self.M = np.zeros((dim_z + dim_x, dim_z + dim_x))

        # copy prior and posterior
        self.x_prior = np.copy(self.x)
        self._P1_2_prior = np.copy(self._P1_2)
        self.x_post = np.copy(self.x)
        self._P1_2_post = np.copy(self._P1_2)

    def update(self, z, R2=None):
        """
        Add a new measurement (z) to the kalman filter. If z is None, nothing
        is changed.

        Parameters
        ----------

        z : np.array
            measurement for this update.

        R2 : np.array, scalar, or None
            Sqrt of meaaurement noize. Optionally provide to override the
            measurement noise for this one call, otherwise  self.R2 will
            be used.
        """

        if z is None:
            self.z = np.array([[None] * self.dim_z]).T
            self.x_post = self.x.copy()
            self._P1_2_post = np.copy(self._P1_2)
            return

        if R2 is None:
            R2 = self._R1_2
        elif np.isscalar(R2):
            R2 = eye(self.dim_z) * R2

        # rename for convienance
        dim_z = self.dim_z
        M = self.M

        M[0:dim_z, 0:dim_z] = R2.T
        M[dim_z:, 0:dim_z] = dot(self.H, self._P1_2).T
        M[dim_z:, dim_z:] = self._P1_2.T

        _, r_decomp = qr(M)
        self.S1_2 = r_decomp[0:dim_z, 0:dim_z].T
        self.SI1_2 = pinv(self.S1_2)
        self.K = dot(r_decomp[0:dim_z, dim_z:].T, self.SI1_2)

        # y = z - Hx
        # error (residual) between measurement and prediction
        self.y = z - dot(self.H, self.x)

        # x = x + Ky
        # predict new x with residual scaled by the kalman gain
        self.x += dot(self.K, self.y)
        self._P1_2 = r_decomp[dim_z:, dim_z:].T

        self.z = deepcopy(z)
        self.x_post = self.x.copy()
        self._P1_2_post = np.copy(self._P1_2)

    def predict(self, u=0):
        """
        Predict next state (prior) using the Kalman filter state propagation
        equations.

        Parameters
        ----------

        u : np.array, optional
            Optional control vector. If non-zero, it is multiplied by B
            to create the control input into the system.
        """

        # x = Fx + Bu
        self.x = dot(self.F, self.x) + dot(self.B, u)

        # P = FPF' + Q
        _, P2 = qr(np.hstack([dot(self.F, self._P1_2), self._Q1_2]).T)
        self._P1_2 = P2[: self.dim_x, : self.dim_x].T

        # copy prior
        self.x_prior = np.copy(self.x)
        self._P1_2_prior = np.copy(self._P1_2)

    def residual_of(self, z):
        """returns the residual for the given measurement (z). Does not alter
        the state of the filter.
        """

        return z - dot(self.H, self.x)

    def measurement_of_state(self, x):
        """Helper function that converts a state into a measurement.

        Parameters
        ----------

        x : np.array
            kalman state vector

        Returns
        -------

        z : np.array
            measurement corresponding to the given state
        """
        return dot(self.H, x)

    @property
    def Q(self):
        """Process uncertainty"""
        return dot(self._Q1_2, self._Q1_2.T)

    @property
    def Q1_2(self):
        """Sqrt Process uncertainty"""
        return self._Q1_2

    @Q.setter
    def Q(self, value):
        """Process uncertainty"""
        self._Q = value
        self._Q1_2 = cholesky(self._Q, lower=True)

    @property
    def P(self):
        """covariance matrix"""
        return dot(self._P1_2, self._P1_2.T)

    @property
    def P_prior(self):
        """covariance matrix of the prior"""
        return dot(self._P1_2_prior, self._P1_2_prior.T)

    @property
    def P_post(self):
        """covariance matrix of the posterior"""
        return dot(self._P1_2_prior, self._P1_2_prior.T)

    @property
    def P1_2(self):
        """sqrt of covariance matrix"""
        return self._P1_2

    @P.setter
    def P(self, value):
        """covariance matrix"""
        self._P = value
        self._P1_2 = cholesky(self._P, lower=True)

    @property
    def R(self):
        """measurement uncertainty"""
        return dot(self._R1_2, self._R1_2.T)

    @property
    def R1_2(self):
        """sqrt of measurement uncertainty"""
        return self._R1_2

    @R.setter
    def R(self, value):
        """measurement uncertainty"""
        self._R = value
        self._R1_2 = cholesky(self._R, lower=True)

    @property
    def S(self):
        """system uncertainty (P projected to measurement space)"""
        return dot(self.S1_2, self.S1_2.T)

    @property
    def SI(self):
        """inverse system uncertainty (P projected to measurement space)"""
        return dot(self.SI1_2.T, self.SI1_2)

    def __repr__(self):
        return "\n".join(
            [
                "SquareRootKalmanFilter object",
                pretty_str("dim_x", self.dim_x),
                pretty_str("dim_z", self.dim_z),
                pretty_str("dim_u", self.dim_u),
                pretty_str("x", self.x),
                pretty_str("P", self.P),
                pretty_str("F", self.F),
                pretty_str("Q", self.Q),
                pretty_str("R", self.R),
                pretty_str("H", self.H),
                pretty_str("K", self.K),
                pretty_str("y", self.y),
                pretty_str("S", self.S),
                pretty_str("SI", self.SI),
                pretty_str("M", self.M),
                pretty_str("B", self.B),
            ]
        )

P property writable

covariance matrix

P1_2 property

sqrt of covariance matrix

P_post property

covariance matrix of the posterior

P_prior property

covariance matrix of the prior

Q property writable

Process uncertainty

Q1_2 property

Sqrt Process uncertainty

R property writable

measurement uncertainty

R1_2 property

sqrt of measurement uncertainty

S property

system uncertainty (P projected to measurement space)

SI property

inverse system uncertainty (P projected to measurement space)

measurement_of_state(x)

Helper function that converts a state into a measurement.

Parameters:

Name Type Description Default
x array

kalman state vector

 required

Returns:

Name Type Description
z array

measurement corresponding to the given state
Source code in bayesian_filters/kalman/square_root.py 
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
def measurement_of_state(self, x):
    """Helper function that converts a state into a measurement.

    Parameters
    ----------

    x : np.array
        kalman state vector

    Returns
    -------

    z : np.array
        measurement corresponding to the given state
    """
    return dot(self.H, x)

predict(u=0)

Predict next state (prior) using the Kalman filter state propagation equations.

Parameters:

Name Type Description Default
u array

Optional control vector. If non-zero, it is multiplied by B to create the control input into the system.

 0
Source code in bayesian_filters/kalman/square_root.py 
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
def predict(self, u=0):
    """
    Predict next state (prior) using the Kalman filter state propagation
    equations.

    Parameters
    ----------

    u : np.array, optional
        Optional control vector. If non-zero, it is multiplied by B
        to create the control input into the system.
    """

    # x = Fx + Bu
    self.x = dot(self.F, self.x) + dot(self.B, u)

    # P = FPF' + Q
    _, P2 = qr(np.hstack([dot(self.F, self._P1_2), self._Q1_2]).T)
    self._P1_2 = P2[: self.dim_x, : self.dim_x].T

    # copy prior
    self.x_prior = np.copy(self.x)
    self._P1_2_prior = np.copy(self._P1_2)

residual_of(z)

returns the residual for the given measurement (z). Does not alter the state of the filter.

Source code in bayesian_filters/kalman/square_root.py 
248
249
250
251
252
253
def residual_of(self, z):
    """returns the residual for the given measurement (z). Does not alter
    the state of the filter.
    """

    return z - dot(self.H, self.x)

update(z, R2=None)

Add a new measurement (z) to the kalman filter. If z is None, nothing is changed.

Parameters:

Name Type Description Default
z array

measurement for this update.

 required
R2 np.array, scalar, or None

Sqrt of meaaurement noize. Optionally provide to override the measurement noise for this one call, otherwise self.R2 will be used.

 None
Source code in bayesian_filters/kalman/square_root.py 
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
def update(self, z, R2=None):
    """
    Add a new measurement (z) to the kalman filter. If z is None, nothing
    is changed.

    Parameters
    ----------

    z : np.array
        measurement for this update.

    R2 : np.array, scalar, or None
        Sqrt of meaaurement noize. Optionally provide to override the
        measurement noise for this one call, otherwise  self.R2 will
        be used.
    """

    if z is None:
        self.z = np.array([[None] * self.dim_z]).T
        self.x_post = self.x.copy()
        self._P1_2_post = np.copy(self._P1_2)
        return

    if R2 is None:
        R2 = self._R1_2
    elif np.isscalar(R2):
        R2 = eye(self.dim_z) * R2

    # rename for convienance
    dim_z = self.dim_z
    M = self.M

    M[0:dim_z, 0:dim_z] = R2.T
    M[dim_z:, 0:dim_z] = dot(self.H, self._P1_2).T
    M[dim_z:, dim_z:] = self._P1_2.T

    _, r_decomp = qr(M)
    self.S1_2 = r_decomp[0:dim_z, 0:dim_z].T
    self.SI1_2 = pinv(self.S1_2)
    self.K = dot(r_decomp[0:dim_z, dim_z:].T, self.SI1_2)

    # y = z - Hx
    # error (residual) between measurement and prediction
    self.y = z - dot(self.H, self.x)

    # x = x + Ky
    # predict new x with residual scaled by the kalman gain
    self.x += dot(self.K, self.y)
    self._P1_2 = r_decomp[dim_z:, dim_z:].T

    self.z = deepcopy(z)
    self.x_post = self.x.copy()
    self._P1_2_post = np.copy(self._P1_2)