State estimation is essential in various engineering and scientific domains where precise knowledge of a system's full-state, state---such as its position, velocity, and acceleration---is essential but directly unobservable due to limitations in measurement availability and accuracy. For instance, in autonomous vehicle navigation, accurately estimating the vehicle's position, orientation, and velocity in real-time is vital to ensure safety and operational efficiency, but direct measurements for the full state may not be available. State estimation helps to fill the gaps left by imperfect sensors and an incomplete set of measurements by providing a statistically optimal estimate based on models of the system dynamics and measurement processes. This approach is key in managing the inherent uncertainties and noise in measurements, allowing for more reliable and accurate system control and decision-making
The Kalman Filter and its variants form a family of algorithms underpinned by the principles of predictive correction, optimality (for linear systems), and recursion. The filters operates in two main phases: prediction and update. In the prediction phase, they use the system's previous state and control inputs to produce an estimate of the current state. In the update phase, they incorporate the new measurement data to refine this estimate. The beauty of the Kalman Filter lies in its recursive nature, allowing it to run in real-time and be computationally efficient, making it ideal for many practical applications. Extensions of the Kalman Filter, like the Extended and Unscented Kalman Filters, adapt these principles to handle non-linear systems by using different linearization techniques or sampling methods.
In this tutorial we demonstrate how the Kalman Filter and its variants can be easily constructed in Collimator. For a continuous-time Pendulum plant, we demonstrate the convenience of creating such filters by passing the entire plant to the convenience functions available in Collimator, while the linearization and discretization of the plant is automatically handled by Collimator.
The Pendulum plant is well described in our previous tutorial on Linear Quadratic Regulators (LQR), which also shows how to linearize the continuous-time plant. We recap the dynamics here. Consider a pendulum with a rigid rod of length $ L $ and a point mass $ m $ at the end (see schematic shown below). The pendulum is actuated by a torque $ u $ applied at the pivot. The damping coefficient is $b$.
The dynamics of this pendulum are given by the following with $ \omega = \dot{\theta} $ as the angular velocity and state $\mathbf{x} = [\theta \quad \omega]^\text{T}$:
\begin{equation}
\dot{\mathbf{x}} =
\begin{bmatrix}
\dot{\theta} \\ \dot{\omega}
\end{bmatrix} =
\begin{bmatrix}
\omega \\ -\dfrac{g}{L} \sin(\theta) - \dfrac{b}{m L^2} \omega + \dfrac{u}{m L^2}
\end{bmatrix}. \tag{1}
\end{equation}
We also assume that only the angular position $\theta$ is available as the measurement $\mathbf{y}$:
$$\mathbf{y} = \theta.$$
Note that for the LQR tutorial, it was assumed that the full state $\mathbf{x}$ of the Pendulum, i.e. both its angular position $\theta$ and angular velocity $\omega$ were available to the LQR. However, in the above setup for the Pendulum, only $\theta$ is measured. This is where Kalman Filter plays a critical role---with noisy measurements of a part (or a transformation) of the state $\mathbf{y}$, it provides optimal estimates for the full state $\mathbf{x}$, which in-turn may be used by any control strategy that requires full state, for example the LQR, Model Predictive Control, etc.
Consider the following linear discrete-time system with state vector $\mathbf{x}[k]$, measurement vector $\mathbf{y}[k]$, and control vector $\mathbf{u}[k]$ :
\begin{align}
\mathbf{x}[k+1] &= \mathbf{A}_d \mathbf{x}[k] + \mathbf{B}_d \mathbf{u}[k] + \mathbf{G}_d \mathbf{w}[k],\\[5pt]
\mathbf{y}[k] &= \mathbf{C}_d \mathbf{x}[k] + \mathbf{D}_d \mathbf{u}[k] +\mathbf{v}[k], \tag{2}
\end{align}
with
\begin{align}
\mathbf{w}[k] &\sim \mathcal{N}(\mathbf{0}, \mathbf{Q_d}),\\[5pt]
\mathbf{v}[k] &\sim \mathcal{N}(\mathbf{0}, \mathbf{R_d}),\\
\end{align}
where,
\begin{align}
\mathbf{A}_d & \quad \text{: State transition matrix}\\[5pt]
\mathbf{B}_d & \quad \text{: Input matrix}\\[5pt]
\mathbf{C}_d & \quad \text{: Output matrix}\\[5pt]
\mathbf{D}_d & \quad \text{: Feedthrough matrix}\\[5pt]
\mathbf{G}_d & \quad \text{: Process noise matrix}\\[5pt]
\mathbf{Q}_d & \quad \text{: Process noise covariance matrix}\\[5pt]
\mathbf{R} & \quad \text{: Measurement noise covariance matrix}\\[5pt]
\mathbf{w}[k] & \quad \text{: Process noise }\\[5pt]
\mathbf{v}[k] & \quad \text{: Measurement noise}.\\[5pt]
\end{align}
For the above system, the Kalman Filter recursion equations are as follows:
1. Prediction/Propagation step:
\begin{align}
\mathbf{\hat{x}}_{k|k-1} &= \mathbf{A}_d \mathbf{\hat{x}}_{k-1|k-1} + \mathbf{B}_d \mathbf{u}[k-1] &\quad \text{(a) Predicted (a priori) state estimate} \\[5pt]
\mathbf{P}_{k|k-1} &= \mathbf{A}_d \mathbf{P}_{k-1|k-1} \mathbf{A}_d^T + \mathbf{G}_d \mathbf{Q}_d \mathbf{G}_d^T &\quad \text{(b) Predicted (a priori) estimate covariance}
\end{align}
2. Update/Correction step:
\begin{align}
\mathbf{K}_k &= \mathbf{P}_{k|k-1} \mathbf{C}_d^T (\mathbf{C}_d \mathbf{P}_{k|k-1} \mathbf{C}_d^T + \mathbf{R}_d)^{-1} &\quad \text{(c) Kalman gain} \\[5pt]
\mathbf{\hat{x}}_{k|k} &= \mathbf{\hat{x}}_{k|k-1} + \mathbf{K}_k (\mathbf{y}[k] - \mathbf{C}_d \mathbf{\hat{x}}_{k|k-1} - \mathbf{D}_d \mathbf{u}[k]) &\quad \text{(d) Updated (a posteriori) state estimate} \\[5pt]
\mathbf{P}_{k|k} &= (\mathbf{I} - \mathbf{K}_k \mathbf{C}_d) \mathbf{P}_{k|k-1} &\quad \text{(e) Updated (a posteriori) estimate covariance}
\end{align}
where:
Through the above states, starring from an initial state estimate $\mathbf{\hat{x}}_0 = \mathbf{\hat{x}}_{0|0}$ and $\mathbf{\hat{P}}_0 = \mathbf{P}_{0|0}$, the Kalman Filter recursively generates optimal estimates for the state.
Note that the above recursion for the Kalman Filter works on a discrete-time linear representation of the plant, eq (2). In our case, the Pendulum plant of eq (1) is continuous-time. Furthermore, it is also nonlinear. For the former, any discretization strategy such as Euler, Zero-order hold, Runge-Kutta methods, etc. may be utilized. For the latter, a linearized version of the continuous plant can be obtained at equilibrium points. This aspect has been covered in the previous LQR tutorial. For Kalman Filters, Collimator provides convenience utility method to directly take the continuous-time plant as an input, and automatically perform its linearization and discretization. Let's see this in action below.
We begin by importing the relevant libraries to create the plant, along with the Kalman Filter and its variants.
We create a LeafSystem implementing the Pendulum plant, eq (1), ensuring that its output is only the partial state measurement of $\theta$. You may build this plant with primitive blocks as well (see for example, this tutorial).
Next, we create the Pendulum plant with disturbances. We introduce process noise in the above continuous plant by making the control torque input $u$ noisy by adding a zero-mean white noise of covariance $\mathbf{Q}$ to it. We also introduce measurement noise by making adding a zero-mean white noise of covariance $\mathbf{R}$ to the output of the above plant. Collimator provides a [.code]WhiteNoise[.code] block, which can generate continuous-time noise with Identity covariance matrix. To obtain noise with any given covariance $\mathbf{N}$, we can pre-multiply the output of the [.code]WhiteNoise[.code] block by the Cholesky factorization of $\mathbf{N}$. This subdiagram is abstracted in the [.code]make_disturbance_from_noise_covariance[.code] function below.
Subsquently, we modify the noiseless Pendulum plant we created previously by adding the process and measurement noises. This is implemented in the function below.
The above function will create a continuous Pendulum plant with noise/disturbances. However, it is non-linear. For Kalman Filter, we need a linear plant, which can be obtained by linearization of the Pendlum around its equilibrium (fixed) point $[\bar{\mathbf{x}}, \bar{\mathbf{u}}]$. While the Collimator utility will automatically perform linearization, the resulting Kalman Filter will be in the coordinates relative to the equilibrium points. To address this, we create the following [.code]make_estimator_diagram[.code], which can perform the transformation back to the global coordinates. In the examples presented below, this is not strictly necessary as the equilibrium point considered is $[\bar{\mathbf{x}}=\mathbf{0}, \bar{\mathbf{u}}=\mathbf{0}]$ and the plant output is also $\mathbf{0}$. Thus the transformations are identity. However, the mechanism is included here for generality.
With the above utilities, we are now ready to create the Kalman Filter. Let's create the configuration first. We need to specify the continuous-time process and noise matrices $\mathbf{Q}$ and $\mathbf{R}$, and the equilirium point (down orientation of the Pendulum) for the automatic linearization of the plant. We also need to specify the initial state and covariance matrices for the Kalman Filter state estimates.
We can now construct the Kalman Filter through the [.code]KalmanFilter.for_continuous_plant[.code] utility, to which we can pass the continuous-time plant. For other ways to use the [.code]KalmanFilter[.code], please see documentation.
The above call linearizes the continuous plant at the provided equilibrium points [.code](x_eq, u_eq)[.code], and returns [.code]y_eq[.code] (plant output at equilibrium point) and the Kalman Filter [.code]kf_bar[.code] for the linearized and discretized plant. The [.code]kf_bar[.code] filter is in the coordinates relative the equilibrium points. We can use the [.code]make_estimator_diagram[.code] utility to transform [.code]kf_bar[.code] into global coordinates, and wire it up with the noisy plant.
A schematic of the diagram we are building in code is shown below
Next, we can simulate the system and plot the results.
We notice that despite the significant noise in the measurement of $\theta$, the Kalman Filter provides a good estimate of both $\theta$ and $\omega$.
The Infinite Horizon Kalman Filter (IHKF), also known as the Steady-State Kalman Filter, is an adaptation of the traditional Kalman Filter presented above, where the filter gains converge to constant values as time progresses to infinity. This filter is particularly useful in systems where the dynamics, noise characteristics, and observation models are constant over time.
For the same system as eq (1), once the system reaches a steady state, the recursion equations stabilize to constant values:
1. Steady-State Kalman Gain:
\begin{align}
\mathbf{K}_\infty &= \mathbf{P}_\infty \mathbf{C}_d^T (\mathbf{C}_d \mathbf{P}_\infty \mathbf{C}_d^T + \mathbf{R}_d)^{-1} &\quad \text{(a) Steady-State Kalman gain}
\end{align}
2. Steady-State Error Covariance:
\begin{align}
\mathbf{P}_\infty &= \mathbf{A}_d \mathbf{P}_\infty \mathbf{A}_d^T - \mathbf{A}_d \mathbf{P}_\infty \mathbf{C}_d^T (\mathbf{C}_d \mathbf{P}_\infty \mathbf{C}_d^T + \mathbf{R}_d)^{-1} \mathbf{C}_d \mathbf{P}_\infty \mathbf{A}_d^T + \mathbf{G}_d \mathbf{Q}_d \mathbf{G}_d^T &\quad \text{(b) Steady-State error covariance}
\end{align}
These steady-state values are derived by solving the above Discrete-Time Algebraic Riccati Equation (DARE), ensuring that the filter no longer relies on iterative updates for $\mathbf{P}_k$ and $\mathbf{K}_k$. This yields computational efficiency and consistent performance in long-running systems.
With $\mathbf{K}_\infty$ and $\mathbf{P}_\infty$, the Infinite Horizon Kalman Filter provides a robust approach for systems operating under steady conditions, simplifying implementation and reducing computation in environments where rapid recalculations are impractical or unnecessary. The Collimator [.code]InfiniteHorizonKalmanFilter.for_continuous_plant[.code] utility can automatically compute the steady state gain and covariances.
The implementation of IHKF is similar to that presented above for the traditional Kalman Filter. We instantiate the filter with the [.code]InfiniteHorizonKalmanFilter.for_continuous_plant[.code] utility in an identical manner, except that we do not need to provide the initial value for the covariance matrix.
We observe that the IHKF has slightly poorer performance relative to the traditional Kalman Filter. However, it is significantly more computationally efficient.
The continuous-time Infinite Horizon Kalman Filter (also known as the Kalman-Bucy filter) is an extension of the Kalman Filter to continuous-time systems where the filter gains converge to constant values as time approaches infinity. Since the filter works with continuous-time systems, plant discretization is not necessary.
Consider, a continuous-time system as follows:
\begin{align}
d\mathbf{x}(t) &= \mathbf{A}_c \mathbf{x}(t) dt + \mathbf{B}_c \mathbf{u}(t) dt + \mathbf{G}_c d\mathbf{w}(t),\\[5pt]
d\mathbf{y}(t) &= \mathbf{C}_c \mathbf{x}(t) dt + \mathbf{D}_c \mathbf{u}(t) dt + d\mathbf{v}(t),
\end{align}
where
\begin{align}
\mathbf{w}(t) &\sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_c dt),\\[5pt]
\mathbf{v}(t) &\sim \mathcal{N}(\mathbf{0}, \mathbf{R}_c dt),\\
\end{align}
and $\mathbf{A}_c, \mathbf{B}_c, \mathbf{C}_c, \mathbf{D}_c, \mathbf{G}_c, \mathbf{Q}_c, \mathbf{R}_c, \mathbf{w}(t), \mathbf{v}(t)$ represent the continuous-time system matrices and noise processes.
In the steady state, the filter achieves a fixed structure for the filter gain and error covariance, which are computed as follows:
1. Steady-State Kalman Gain:
\begin{align}
\mathbf{K}_\infty &= \mathbf{P}_\infty \mathbf{C}_c^T \mathbf{R}_c^{-1} &\quad \text{(a) Steady-State Kalman gain}
\end{align}
2. Steady-State Error Covariance:
\begin{align}
\mathbf{0} &= \mathbf{P}_\infty \mathbf{A}_c^T + \mathbf{A}_c \mathbf{P}_\infty + \mathbf{G}_c \mathbf{Q}_c \mathbf{G}_c^T - \mathbf{P}_\infty \mathbf{C}_c^T \mathbf{R}_c^{-1} \mathbf{C}_c \mathbf{P}_\infty &\quad \text{(b) Steady-State error covariance}
\end{align}
These steady-state values are derived by solving the above Continuous-Time Algebraic Riccati Equation (CARE). This filter offers several advantages, including reduced computational demands and improved stability over long operational periods, as the filter's performance does not degrade with time.
Utilizing $\mathbf{K}_\infty$ and $\mathbf{P}_\infty$, the Infinite Horizon Continuous-Time Kalman Filter ensures consistent and efficient state estimation for systems subjected to continuous inputs and observations. The estimator for the state $\hat{\mathbf{x}}$ becomes an ODE:
$$ \frac{d \hat{\mathbf{x}}}{dt} = \mathbf{(A-K_{\infty}C)} \hat{\mathbf{x}} + \mathbf{(B-K_{\infty}D)u} + \mathbf{K_{\infty}y}$$
The Collimator [.code]ContinuousTimeInfiniteHorizonKalmanFilter.for_continuous_plant[.code] utility can automatically compute the steady state gain and covariances.
Similarly to as we did before for other filters, we instantiate the filter with the [.code]ContinuousTimeInfiniteHorizonKalmanFilter.for_continuous_plant[.code] utility, except that we do not need to provide the the discrete-time interval and discretization parameters.
As before, this filter too provides good estimates of $\theta$ and $\omega$.
So far, we worked with linear variants of the Kalman Filter. Since our plant is nonlinear, we linearized the plant around the 'down' orientation of the Pendulum plant. Consequently, our linearized model is only valid in the region around the equilibrium point, and we were restricted to this region (reflected in our choice of the initial condition not being too far away from the equilibrium point, ensuring that the plant remains in a region where linearization is valid). If we need to perform state estimation in regions far from the equilibrium point, we need to employ nonlinear variants in the Kalman Filter family, which do not require a linear plant. These extensions are presented next.
The two primary nonlinear variants of the Kalman Filter are the Extended Kalman Filter (EKF) and the Unscented Kalman Filter (UKF).
EKF is used to estimate the state of a nonlinear system, and it linearizes the plant about the current estimate to apply the traditional Kalman Filter equations.
Consider the following nonlinear discrete-time system with state vector $\mathbf{x}[k]$, measurement vector $\mathbf{y}[k]$, and control vector $\mathbf{u}[k]$:
\begin{align}
\mathbf{x}[k+1] &= \mathbf{f}(\mathbf{x}[k], \mathbf{u}[k]) + \mathbf{G}\mathbf{w}[k],\\[5pt]
\mathbf{y}[k] &= \mathbf{h}(\mathbf{x}[k], \mathbf{u}[k]) + \mathbf{v}[k],
\end{align}
with
\begin{align}
\mathbf{w}[k] &\sim \mathcal{N}(\mathbf{0}, \mathbf{Q}),\\[5pt]
\mathbf{v}[k] &\sim \mathcal{N}(\mathbf{0}, \mathbf{R}),\\
\end{align}
where,
\begin{align}
\mathbf{f} & \quad \text{: State transition function}\\[5pt]
\mathbf{h} & \quad \text{: Measurement function}\\[5pt]
\mathbf{Q} & \quad \text{: Process noise covariance matrix}\\[5pt]
\mathbf{R} & \quad \text{: Measurement noise covariance matrix}\\[5pt]
\mathbf{w}[k] & \quad \text{: Process noise}\\[5pt]
\mathbf{v}[k] & \quad \text{: Measurement noise}.\\[5pt]
\end{align}
For the above system, the Extended Kalman Filter recursion equations are as follows:
1. Prediction/Propagation step:
\begin{align}
\mathbf{\hat{x}}_{k|k-1} &= \mathbf{f}(\mathbf{\hat{x}}_{k-1|k-1}, \mathbf{u}[k-1]) &\quad \text{(a) Predicted (a priori) state estimate} \\[5pt]
\mathbf{F}_k &= \left.\frac{\partial \mathbf{f}}{\partial \mathbf{x}}\right|_{\mathbf{\hat{x}}_{k-1|k-1}, \mathbf{u}[k-1]} &\quad \text{(b) Jacobian of the state transition function w.r.t. state} \\[5pt]
\mathbf{P}_{k|k-1} &= \mathbf{F}_k \mathbf{P}_{k-1|k-1} \mathbf{F}_k^T + \mathbf{G Q G}^T &\quad \text{(c) Predicted (a priori) estimate covariance}
\end{align}
2. Update/Correction step:
\begin{align}
\mathbf{H}_k &= \left.\frac{\partial \mathbf{h}}{\partial \mathbf{x}}\right|_{\mathbf{\hat{x}}_{k|k-1}} &\quad \text{(d) Jacobian of the measurement function w.r.t. state} \\[5pt]
\mathbf{S}_k &= \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T + \mathbf{R} &\quad \text{(e) Innovation covariance} \\[5pt]
\mathbf{K}_k &= \mathbf{P}_{k|k-1} \mathbf{H}_k^T \mathbf{S}_k^{-1} &\quad \text{(f) Kalman gain} \\[5pt]
\mathbf{\hat{x}}_{k|k} &= \mathbf{\hat{x}}_{k|k-1} + \mathbf{K}_k (\mathbf{y}[k] - h(\mathbf{\hat{x}}_{k|k-1})) &\quad \text{(g) Updated (a posteriori) state estimate} \\[5pt]
\mathbf{P}_{k|k} &= (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1} &\quad \text{(h) Updated (a posteriori) estimate covariance}
\end{align}
where:
The Extended Kalman Filter starts with an initial state estimate $\mathbf{\hat{x}}_0 = \mathbf{\hat{x}}_{0|0}$ and $\mathbf{\hat{P}}_0 = \mathbf{P}_{0|0}$, and recursively generates optimal estimates for the state using linear approximations at each step.
We now choose large large angles as the nonlinear extensions should be able to handle nonlinear plants in any region.
Similarly to linear Kalman Filters, Collimator provides convenience functions to create the EKF automatically through the [.code]ExtendedKalmanFilter.for_continuous_plant[.code] utility. Here, we may also pass time functions representing time-varying $\mathbf{G, Q, R}$.
Despite large angles and large difference in the initial guessed value of the pendulum, the EKF is able to remarkably provide excellent estimates of both $\theta$ and $\omega$.
The UKF uses the Unscented Transform to approximate the state distribution through a deterministic sampling approach. This method captures the true mean and covariance of the state distribution more accurately, especially in highly nonlinear systems. This approach eliminates the need for computing Jacobians, as in the EKF.
For the the same nonlinear system presented above for the EKF, the Unscented Kalman Filter recursion equations are as follows:
1. Sigma Point Generation and Propagation:
\begin{align}
\mathbf{X}_k^- &= \mathbf{f}(\mathbf{X}_{k-1}^+)\\[5pt]
\end{align}
2. Prediction:
\begin{align}
\mathbf{\hat{x}}_{k|k-1} &= \text{Weighted mean of } \mathbf{X}_k^-\\[5pt]
\mathbf{P}_{k|k-1} &= \text{Weighted covariance of } \mathbf{X}_k^- + \mathbf{GQG}^T\\[5pt]
\end{align}
3. Update:
\begin{align}
\mathbf{Y}_k &= \mathbf{h}(\mathbf{X}_k^-)\\[5pt]
\mathbf{\hat{y}}_k &= \text{Weighted mean of } \mathbf{Y}_k\\[5pt]
\mathbf{P}_{yy,k} &= \text{Weighted covariance of } \mathbf{Y}_k + \mathbf{R}\\[5pt]
\mathbf{P}_{xy,k} &= \text{Weighted cross covariance of } \mathbf{X}_k^- \text{ and } \mathbf{Y}_k\\[5pt]
\mathbf{K}_k &= \mathbf{P}_{xy,k} \mathbf{P}_{yy,k}^{-1}\\[5pt]
\mathbf{\hat{x}}_{k|k} &= \mathbf{\hat{x}}_{k|k-1} + \mathbf{K}_k(\mathbf{y}[k] - \mathbf{\hat{y}}_k)\\[5pt]
\mathbf{P}_{k|k} &= \mathbf{P}_{k|k-1} - \mathbf{K}_k \mathbf{P}_{yy,k} \mathbf{K}_k^T\\[5pt]
\end{align}
where:
The Unscented Kalman Filter starts with an initial state estimate $\mathbf{\hat{x}}_0 = \mathbf{\hat{x}}_{0|0}$ and $\mathbf{\hat{P}}_0 = \mathbf{P}_{0|0}$, and recursively generates optimal estimates for the state using the Unscented Transform at each step.
The UKF has a similar API to EKF, through the [.code]UnscentedKalmanFilter.for_continuous_plant[.code] utility. This is demonstrated below.
Like EKF, the UKF also provides good state estimates in nonlinear regions.
For fun, we can also animate the results to visualize the UKF estimation:
.png){kind=small}
.png){kind=small}
.png){kind=small}
.png){kind=small}
.png){kind=small}
