|
| 1 | +from typing_extensions import Annotated, Doc |
| 2 | +from pydantic import BaseModel, Field, PrivateAttr |
| 3 | + |
| 4 | + |
| 5 | +class KalmanFilter(BaseModel): |
| 6 | + r"""One-dimensional Kalman filter for time series data. |
| 7 | +
|
| 8 | + This implementation uses a simple 1D state-space model: |
| 9 | +
|
| 10 | + $$ |
| 11 | + \begin{align} |
| 12 | + x_t &= x_{t-1} + w_t, \quad w_t \sim \mathcal{N}(0, Q) \\ |
| 13 | + z_t &= x_t + v_t, \quad v_t \sim \mathcal{N}(0, R) |
| 14 | + \end{align} |
| 15 | + $$ |
| 16 | +
|
| 17 | + The Kalman filter estimates the hidden state $x_t$ given noisy measurements $z_t$. |
| 18 | + The ratio $Q/R$ determines the smoothing behavior. |
| 19 | + """ |
| 20 | + |
| 21 | + R: float = Field(default=1.0, gt=0.0, description="Measurement noise covariance") |
| 22 | + Q: float = Field(default=0.01, gt=0.0, description="Process noise covariance") |
| 23 | + |
| 24 | + _x: float | None = PrivateAttr(default=None) # State estimate |
| 25 | + _P: float = PrivateAttr(default=1.0) # Error covariance |
| 26 | + _K: float = PrivateAttr(default=0.0) # Kalman Gain |
| 27 | + |
| 28 | + def value(self) -> float | None: |
| 29 | + """Get the most recent smoothed value (state estimate), if available.""" |
| 30 | + return self._x |
| 31 | + |
| 32 | + def update( |
| 33 | + self, |
| 34 | + value: Annotated[float, Doc("New noisy measurement to update the filter")], |
| 35 | + ) -> float: |
| 36 | + """Update the filter with a new value and return the smoothed result.""" |
| 37 | + # Initialize on first update |
| 38 | + if self._x is None: |
| 39 | + self._x = value |
| 40 | + self._P = self.R |
| 41 | + return value |
| 42 | + |
| 43 | + # Prediction step |
| 44 | + # x_pred = x_prev (Random walk model) |
| 45 | + # P_pred = P_prev + Q |
| 46 | + x_pred = self._x |
| 47 | + P_pred = self._P + self.Q |
| 48 | + |
| 49 | + # Update step |
| 50 | + # K = P_pred / (P_pred + R) |
| 51 | + # x_new = x_pred + K * (measurement - x_pred) |
| 52 | + # P_new = (1 - K) * P_pred |
| 53 | + K = P_pred / (P_pred + self.R) |
| 54 | + x_new = x_pred + K * (value - x_pred) |
| 55 | + P_new = (1 - K) * P_pred |
| 56 | + |
| 57 | + # Update state |
| 58 | + self._x = x_new |
| 59 | + self._P = P_new |
| 60 | + self._K = K |
| 61 | + |
| 62 | + return x_new |
| 63 | + |
| 64 | + @property |
| 65 | + def error_covariance(self) -> float: |
| 66 | + """Current estimated error covariance.""" |
| 67 | + return self._P |
| 68 | + |
| 69 | + @property |
| 70 | + def kalman_gain(self) -> float: |
| 71 | + """Most recent Kalman gain.""" |
| 72 | + return self._K |
0 commit comments