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

Add discrete-time steady-state Kalman filter #22687

Open
wants to merge 1 commit into
base: master
Choose a base branch
from

Conversation

wei-chen-li
Copy link
Contributor

@wei-chen-li wei-chen-li commented Mar 2, 2025

This PR implements the discrete-time steady-state Kalman filter and Luenberger observer.

For reference, the Kalman filter dynamics is given by:

$$ K = \hat{P} C^\top (C \hat{P} C^\top+ V)^{-1} $$ $$ \hat{x}[n|n] = \hat{x}[n|n-1] + K(y - \hat{y}) $$ $$ \hat{x}[n+1|n] = A \hat{x}[n|n] + B u[n] $$

At steady-state, the dynamics is given by:

$$ \hat{x}[n+1|n] = A \hat{x}[n|n-1] + B u[n] + L(y - \hat{y}) $$

Thus, the observer gain is:

$$ L = A \hat{P} C^\top (C \hat{P} C^\top + V)^{-1} $$


This change is Reviewable

@jwnimmer-tri
Copy link
Collaborator

+@RussTedrake for feature review or delegation, please.

@jwnimmer-tri jwnimmer-tri added the release notes: feature This pull request contains a new feature label Mar 3, 2025
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
release notes: feature This pull request contains a new feature
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants