-class simple_kalman:
- def __init__(self, x_est, P_est, Q, R):
- self.x_est = x_est # Systemzustand
- self.P_est = P_est # Fehlerkovarianz
- self.Q = Q # Systemrauschen
- self.R = R # Varianz des Messfehlers
-
- def run(self, y):
- # Korrektur mit der Messung
- # (1) Berechnung der Kalman Verstärkung
- K = self.P_est/(self.R + self.P_est)
- # (2) Korrektur der Schätzung mit der Messung y
- x = self.x_est + K*(y - self.x_est)
- # (3) Korrektur der Fehlerkovarianzmatrix
- P = (1-K)*self.P_est
- #
- # Prädiktion
- # (1) Prädiziere den Systemzustand
- self.x_est = x
- # (2) Präzidiere die Fehlerkovarianzmatrix
- self.P_est = P + self.Q
-
- return x