Open
Description

For the extended Kalman filter, the state is updated with the state transition function f, and the state uncertainty is updated with the jacobian of the state transition function F. In the predict step in the code, the state and uncertainty is updated using the same matrix F as in a normal Kalman filter, is this correct? Shouldn't the jacobian of the state transition function be calculated first and then update the uncertainty with that (as in the formula in the image)? Maybe I have misunderstood something but I changed this for my tracking algorithm and got better results.
def predict_x(self, u=0):
"""
Predicts the next state of X. If you need to
compute the next state yourself, override this function. You would
need to do this, for example, if the usual Taylor expansion to
generate F is not providing accurate results for you.
"""
self.x = dot(self.F, self.x) + dot(self.B, u)
def predict(self, u=0):
"""
Predict next state (prior) using the Kalman filter state propagation
equations.
Parameters
----------
u : np.array
Optional control vector. If non-zero, it is multiplied by B
to create the control input into the system.
"""
self.predict_x(u)
self.P = dot(self.F, self.P).dot(self.F.T) + self.Q
# save prior
self.x_prior = np.copy(self.x)
self.P_prior = np.copy(self.P)
Metadata
Metadata
Assignees
Labels
No labels