EKF example
-
I would like to use the EKF (extended Kalman filter) block. I have seen a very complicated example via my colleague, but it has too many extraneous parts to be understandable.
Our case is very simple. To give the simplest form, consider a pendulum, theta''[t] + sin[theta] = u[t], where u is a deterministic (known) input signal. We measure theta[t] (has noise) and want to estimate the velocity theta'[t] via the EKF. The measurements are rapid enough, relative to the period, that a simple discrete time EKF (e.g. Euler step) should be fine. So no fancy integration techniques are needed.
Does someone know how to set up such an example for the EKF block (or have something equivalent)? That would help us a lot. Thanks!
-
@bech Hi,
I'm sending an example of using EKF for a linear model. Please take a look at the code in the REXLANG block. Unfortunately, I don't have time to document the example in more detail right now. However, I should have time next week, so I'll send an improved version then.
Best regards,
Jan
EKF.zip -
@Jan-Reitinger Thank you. We are in the process of trying to understand the example better and to start to modify for our case. Any documentation or comments on the supplied example that you have time for would be welcome.
-
@bech Hi,
here is a Markdown file describing the system, which is implemented in the REXLANG block. To view the equations correctly, copy the file's contents into an online editor, such as this one: https://markdownlivepreview.org -
EKF - Linear System Model
This document describes the use of the Extended Kalman Filter (EKF) in the REXYGEN environment for state estimation of a linear system.
System Description
The system is described by the following differential equations:
\begin{aligned} \frac{dx(t)}{dt} &= f(x(t), u(t)) + w(t),\\ y(t) &= h(x(t), u(t)) + v(t),\\ \hat{x}(t) &\sim \mathcal{N}(x, P),\\ w(t) &\sim \mathcal{N}(0, Q),\\ v(t) &\sim \mathcal{N}(0, R). \end{aligned}where:
- $ x(t) $ is the state vector,
- $ u(t) $ is the input vector,
- $ y(t) $ is the output vector,
- $ w(t) $ is the process noise,
- $ v(t) $ is the measurement noise,
- $ \hat{x}(t) $ is the estimated state vector,
- $ x $ is the mean (expected value),
- $ P $ is the state estimation covariance matrix,
- $ Q $ is the process noise covariance matrix,
- $ R $ is the measurement noise covariance matrix.
State Model of the System
The system dynamics are defined by the function $ f(x(t), u(t)) $:
f(x(t), u(t)) = \begin{bmatrix} \frac{dx_1(t)}{dt} \\ \frac{dx_2(t)}{dt} \\ \frac{dx_3(t)}{dt} \\ \frac{dx_4(t)}{dt} \end{bmatrix} = \begin{bmatrix} -x_1(t) + x_2(t) \\ -x_2(t) + x_3(t) \\ -x_3(t) + x_4(t) \\ -x_4(t) + u_1(t) \end{bmatrix}.Jacobian of the State Function
The Jacobian $ f(x, u) $ with respect to the state vector $ x $ is given as:
\frac{df(x(t), u(t))}{dx} = \begin{bmatrix} -1 & 1 & 0 & 0 \\ 0 & -1 & 1 & 0 \\ 0 & 0 & -1 & 1 \\ 0 & 0 & 0 & -1 \end{bmatrix}Output Measurements
The first set of output measurements (which, in this example, depend linearly on the state variables) for input EKF:nz = 1 is defined as:
y(t) = h_1(x(t), u(t)) = \begin{bmatrix} x_1(t) \\ 2 x_2(t) + 0.5 x_4(t) \end{bmatrix},with the corresponding Jacobian:
\frac{dh_1(x(t), u(t))}{dt} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 2 & 0 & 0.5 \\ \end{bmatrix}.The second set of output measurements (for input EKF:nz = 2) is not used.
The system description is programmed into the REXLANG block.
This description serves as a basis for implementing the EKF in REXYGEN using the EKF function block. Further details can be found in the official documentation: EKF Block in REXYGEN.
-
@Jan-Reitinger I upgraded your example. Now it uses exact reference model both for verification and emulating output measurements to be used as input to the estimator.
However, there is some timing issue here. I use tick=0.02, ntick0=4. It means period of the task is 80 ms. If I go lower with that, I receive overtime in diagnostics. This is odd, because it means that computational load is too high.
It should be much less than that.
Here is my current version:
EKF_John.zip