Skip to content

Predict Step in EKF #520

Open
Open
@JoX00

Description

@JoX00
ekf_issue

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

No one assigned

    Labels

    No labels
    No labels

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions