Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Predict Step in EKF #520

Open
JoX00 opened this issue Dec 18, 2024 · 0 comments
Open

Predict Step in EKF #520

JoX00 opened this issue Dec 18, 2024 · 0 comments

Comments

@JoX00
Copy link

JoX00 commented Dec 18, 2024

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)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant