Using the Kalman filter to determine the derivatives of the measured value

Recently, I was engaged in solving the problem of transferring a state vector from an existing motion model to a special device that forms a navigation signal. The following restrictions existed:

• the motion model approximately periodically sends previously calculated coordinates and velocity of the object with a time stamp in a known format via UDP;
• the simulator of the navigation signal can establish a TCP connection and, through it, receive a state vector that includes, in addition to the coordinates and velocities, accelerations and jerks — derivatives of accelerations or third derivatives of the coordinates;
• at speeds up to 10 ^ 4 m / s, the disturbing acceleration does not exceed 0.001 m / s2;
• coordinates can be considered independent;
• The simulator of the navigation signal should receive a forecast of the state vector at a given point in the future .

The need to calculate accelerations and jerky led me to the idea that the polynomial of the appropriate order should be used for prediction, but the question of determining the polynomial coefficients remained open.

I considered various options, from mathematically incorrect, but intuitively understandable and usually working homegrown algorithms to spline approximation and filtering, and stopped at Kalman filtering. My choice was determined on the one hand by its mathematical correctness, and on the other by a long-standing desire to try Kalman in action.

Python was chosen as a tool for solving the set-specific problem.

A search for “kalman filtering python” showed that there are standard implementations of filterpy and pykalman , and in addition, you can quite simply implement Kalman filtering using the widely used numpy package. Since the script was supposed to work on a Windows machine that does not have an internet connection, I decided to stop at the last version.
')
Based on the scipy cookbook and the Wikipedia article about Kalman filtering, I implemented a test program that performs the required functionality with a predetermined set of input data.

Sources for the third version of python are given below. The designations coincide with those given in the Wikipedia article.

When debugging, I stepped on the following rake: I set the initial value of the covariance matrix to an estimate of the state vector P for zero if the initial values ​​of acceleration and jerk are not equal to zero (they are unknown, remember?). After correcting this error, I received a brilliant coincidence of the original and predicted data.

I used the standard Python profiler to estimate the speed of the algorithm and, first of all, the inversion of the deviation vector covariance matrix S.

Source code of the trial program:

import numpy as np import pylab import profile #   n_iter = 5000 #   dim = 5 #    dim_o = 2 #        def calcF( t ): res = np.identity( dim ) _t = 1.0 for i in range( 1, dim ): _t *= t / i for j in range( 0, dim-i ): res[ j ][ i+j ] = _t return res #       def calcFG( t ): F = np.identity( dim ) G = np.zeros( ( dim, 1 ) ) _t = 1.0 for i in range( 0, dim ): for j in range( 0, dim-i ): F[ j ][ i+j ] = _t if i <= dim_o: G[ dim_o - i ] = _t _t *= t / ( i+1 ) return F, G #      xtruth = np.zeros( ( dim, 1 ) ) xtruth[0][0] = 15.3 xtruth[1][0] = 8.7 xtruth[2][0] = -0.3 xtruth[3][0] = 0.3 xtruth[4][0] = -1.0 #    H = np.zeros( ( dim_o, dim ) ) for i in range( dim_o ): H[i][i] = 1.0 H_t = H.transpose() #     R = 1e-10 * np.identity( dim_o ) # ,     t = 0.1 * np.arange( n_iter ) + np.random.normal( 0.0, 0.02, size=( n_iter, ) ) #    D = 13.3 * 0.05 / 7000 * 2 / 60.0 # ,      x = np.zeros( ( dim, 1 ) ) #     dx = np.zeros( ( dim_o, n_iter ) ) #     P = 10.0 * np.identity( dim ) #   z = np.zeros( ( n_iter, dim_o, 1 ) ) for i in range( 0, n_iter ): z[i] = H.dot( calcF( t[ i ] ) ).dot( xtruth ) #    F  D^2*G*G^T F = np.zeros( ( n_iter, dim, dim ) ) GGt = np.zeros( ( n_iter, dim, dim ) ) for i in range( 1, n_iter ): dt = t[ i ] - t[ i-1 ] F[i], G = calcFG( dt ) GGt[i] = D*D * G.dot( G.transpose() ) #   def calc(): global t, x, P, D, z, H, R, dx for i in range( 1, n_iter ): xpred = F[i].dot( x ) Ppred = F[i].dot( P ).dot( F[i].transpose() ) + GGt[i] y = z[i] - H.dot( xpred ) S = H.dot( Ppred ).dot( H_t ) + R K = Ppred.dot( H_t ).dot( np.linalg.inv( S ) ) x = xpred + K.dot( y ) P = ( np.identity( dim ) - K.dot( H ) ).dot( Ppred ) #   dx[0][i] = y[0][0] / x[0][0] dx[1][i] = y[1][0] / x[1][0] profile.run( 'calc()' ) #   pylab.figure() pylab.plot( t, dx[0], label='x' ) pylab.plot( t, dx[1], label='v' ) pylab.legend() pylab.show() 

Summary

Do not be afraid of mathematics, sometimes it can significantly simplify your life!