I am currently working on an image processing project and I am using a Kalman filter for the algorithm, among other things. However, the computation time of the Kalman filter is very slow compared to other software components, despite the use of numpy.
The predict function is very fast. The update function, however, is not. I think the reason for that could be the calculation of the inverse of the 2x2 matrix np.linalg.inv().
Does anyone have an idea for a faster calculation? Possibly hardcoded or how to rearrange the equation to avoid the inverse calculation?
I also appreciate other comments on how to get the code faster. I may have overlooked something as well.
Thank you very much in advance!
KalmanFilter.py:
class KalmanFilter(object):
def __init__(self, dt, u_x,u_y, std_acc, x_std_meas, y_std_meas):
"""
:param dt: sampling time (time for 1 cycle)
:param u_x: acceleration in x-direction
:param u_y: acceleration in y-direction
:param std_acc: process noise magnitude
:param x_std_meas: standard deviation of the measurement in x-direction
:param y_std_meas: standard deviation of the measurement in y-direction
"""
# Define sampling time
self.dt = dt
# Define the control input variables
self.u = np.array([u_x,u_y])
# Intial State
self.x = np.array([0.,0.,0.,0.,0.,0.]) # x, x', x'', y, y', y''
# Define the State Transition Matrix A
self.A = np.array([[1., self.dt, 0.5*self.dt**2., 0., 0., 0.],
[0., 1., self.dt, 0., 0., 0.],
[0., 0., 1., 0., 0., 0.],
[0., 0., 0., 1., self.dt, 0.5*self.dt**2.],
[0., 0., 0., 0., 1., self.dt],
[0., 0., 0., 0., 0., 1.]])
# Define the Control Input Matrix B
self.B = np.array([[(self.dt**2.)/2., 0.],
[0.,(self.dt**2.)/2.],
[self.dt,0.],
[0.,self.dt]])
# Define Measurement Mapping Matrix
self.H = np.array([[1., 0., 0., 0., 0., 0.],
[0., 0., 0., 1., 0., 0.]])
# Initial Process Noise Covariance
self.Q = np.array([[(self.dt**4.)/4., (self.dt**3.)/2., (self.dt**2.)/2., 0., 0., 0.],
[(self.dt**3.)/2., self.dt**2., self.dt, 0., 0., 0.],
[(self.dt**2.)/2., self.dt, 1., 0., 0., 0.],
[0., 0., 0., (self.dt**4.)/4., (self.dt**3.)/2., (self.dt**2.)/2.],
[0., 0., 0., (self.dt**3.)/2., self.dt**2., self.dt],
[0., 0., 0., (self.dt**2.)/2., self.dt, 1.]]) * std_acc**2.
# Initial Measurement Noise Covariance
self.R = np.array([[x_std_meas**2.,0.],
[0., y_std_meas**2.]])
# Initial Covariance Matrix
self.P = np.array([[500., 0., 0., 0., 0., 0.],
[0., 500., 0., 0., 0., 0.],
[0., 0., 500., 0., 0., 0.],
[0., 0., 0., 500., 0., 0.],
[0., 0., 0., 0., 500., 0.],
[0., 0., 0., 0., 0., 500.]])
# Initial Kalman Gain
self.K = np.zeros((6, 2))
# Initial System uncertainity
self.S = np.zeros((2, 2))
def predict(self):
# Update time state
if self.A is not None and self.u[0] is not None:
self.x = dot(self.A, self.x) + dot(self.B, self.u)
else:
self.x = dot(self.A, self.x)
# Calculate error covariance
self.P = np.dot(np.dot(self.A, self.P), self.A.T) + self.Q
return self.x[0], self.x[3]
def update(self, z):
self.S = np.dot(self.H, np.dot(self.P, self.H.T)) + self.R
# Calculate the Kalman Gain
self.K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(self.S))
self.x = self.x + np.dot(self.K, (z - np.dot(self.H, self.x)))
I = np.eye(self.H.shape[1])
# Update error covariance matrix
helper = I - np.dot(self.K, self.H)
self.P = np.dot(np.dot(helper, self.P), helper.T) + np.dot(self.K, np.dot(self.R, self.K.T))
return self.x[0], self.x[3]
You might get some speedup this way:
def __init__(self, dt, u_x,u_y, std_acc, x_std_meas, y_std_meas):
# your existing code
self.I = np.eye(self.H.shape[1])
And,
def update(self, z):
# you can cut down on 1 dot product if you save P@H.T in an intermediate variable
P_HT = np.dot(self.P, self.H.T)
self.S = np.dot(self.H, P_HT) + self.R
# Calculate the Kalman Gain
self.K = np.dot(P_HT, np.linalg.inv(self.S))
self.x = self.x + np.dot(self.K, (z - np.dot(self.H, self.x)))
# Update error covariance matrix >>> use self.I
helper = self.I - np.dot(self.K, self.H)
self.P = np.dot(np.dot(helper, self.P), helper.T) + np.dot(self.K, np.dot(self.R, self.K.T))
return self.x[0], self.x[3]