We will use a Kalman filter to perform the filtering. The model is a double integrator, i.e., a constant-acceleration model. The state vector is thus $x = [p, v]^T$, where $p$ is the position and $v$ is the velocity. When designing a Kalman filter, we need to specify the noise covariances $R_1$ and $R_2$. While it's often easy to measure the covariance of the measurement noise, $R_2$, it can be quite difficult to know ahead of time what the dynamics noise covariance, $R_1$, should be. In this example, we will use an adaptive filter, where we will increase the dynamics noise covariance if the filter prediction error is too large. However, we first run the filter twice, once with a large $R_1$ and once with a small $R_1$ to illustrate the difference.
y = [[y] for y in Y] # create a vector of vectors for the KF
+plot([Y x], lab=["Measurement" "True state to be tracked"], c=[1 :purple])
We will use a Kalman filter to perform the filtering. The model is a double integrator, i.e., a constant-acceleration model. The state vector is thus $x = [p, v]^T$, where $p$ is the position and $v$ is the velocity. When designing a Kalman filter, we need to specify the noise covariances $R_1$ and $R_2$. While it's often easy to measure the covariance of the measurement noise, $R_2$, it can be quite difficult to know ahead of time what the dynamics noise covariance, $R_1$, should be. In this example, we will use an adaptive filter, where we will increase the dynamics noise covariance if the filter prediction error is too large. However, we first run the filter twice, once with a large $R_1$ and once with a small $R_1$ to illustrate the difference.
y = [[y] for y in Y] # create a vector of vectors for the KF
u = fill([], T) # No inputs in this example :(
# Define the model
@@ -38,7 +38,7 @@
Yh = reduce(hcat, yh)
plot!(Yh', lab="Estimate \$σ_w\$ = $σw")
end
-fig
When $R_1$ is small (controlled by $σ_w$), we get a nice and smooth filter estimate, but this estimate clearly lags behind the true state. When $R_1$ is large, the filter estimate is much more responsive, but it also has a lot of noise.
Below, we will implement an adaptive filter, where we keep the dynamics noise covariance low by default, but increase it if the filter prediction error is too large. We will use a Z-score to determine if the prediction error is too large. The Z-score is defined as the number of standard deviations the prediction error is away from the estimated mean. This time around we use separate correct! and predict! calls, so that we can access the prediction error as well as the prior covariance of the prediction error, $S$. $S$ (or the Cholesky factor $Sᵪ$) will be used to compute the Z-score.
σw = 1e-5 # Set the covariance to a low value by default
+fig
When $R_1$ is small (controlled by $σ_w$), we get a nice and smooth filter estimate, but this estimate clearly lags behind the true state. When $R_1$ is large, the filter estimate is much more responsive, but it also has a lot of noise.
Below, we will implement an adaptive filter, where we keep the dynamics noise covariance low by default, but increase it if the filter prediction error is too large. We will use a Z-score to determine if the prediction error is too large. The Z-score is defined as the number of standard deviations the prediction error is away from the estimated mean. This time around we use separate correct! and predict! calls, so that we can access the prediction error as well as the prior covariance of the prediction error, $S$. $S$ (or the Cholesky factor $Sᵪ$) will be used to compute the Z-score.
σw = 1e-5 # Set the covariance to a low value by default
R1 = σw*[Ts^3/3 Ts^2/2; Ts^2/2 Ts]
kf = KalmanFilter(A, B, C, D, R1, R2)
measure = LowLevelParticleFilters.measurement(kf)
@@ -64,7 +64,7 @@
end
Yh = reduce(hcat, yh)
-plot([Y Yh'], lab=["Measurement" "Adaptive estimate"])
Not too bad! This time the filter estimate is much more responsive during the transition, but exhibits favorable noise properties during the stationary phases. We can also plot the prediction error and the Z-score to see how the filter adapts to the dynamics noise covariance.
plot([es σs], lab=["Prediction error" "Z-score"])
Notice how the prediction errors, that should ideally be centered around zero, remain predominantly negative for a long time interval after the transition. This can be attributed to an overshoot in the velocity state of the estimator, but the rapid decrease of the covariance after the transition makes the filter slow at correcting its overshoot. If we want, we could mitigate this and make the adaptation even more sophisticated by letting the covariance remain large for a while after a transition in operating mode has been detected. Below, we implement a simple version of this, where we use a multiplier $σ_{wt}$ that defaults to 1, but is increase to a very large value of 1000 if a transition is detected. When no transition is detected, $σ_{wt}$ is decreased exponentially back down to 1.
σw = 1e-5 # Set the covariance to a low value by default
+plot([Y Yh'], lab=["Measurement" "Adaptive estimate"])
Not too bad! This time the filter estimate is much more responsive during the transition, but exhibits favorable noise properties during the stationary phases. We can also plot the prediction error and the Z-score to see how the filter adapts to the dynamics noise covariance.
plot([es σs], lab=["Prediction error" "Z-score"])
Notice how the prediction errors, that should ideally be centered around zero, remain predominantly negative for a long time interval after the transition. This can be attributed to an overshoot in the velocity state of the estimator, but the rapid decrease of the covariance after the transition makes the filter slow at correcting its overshoot. If we want, we could mitigate this and make the adaptation even more sophisticated by letting the covariance remain large for a while after a transition in operating mode has been detected. Below, we implement a simple version of this, where we use a multiplier $σ_{wt}$ that defaults to 1, but is increase to a very large value of 1000 if a transition is detected. When no transition is detected, $σ_{wt}$ is decreased exponentially back down to 1.
σw = 1e-5 # Set the covariance to a low value by default
σwt = 1.0
R1 = σw*[Ts^3/3 Ts^2/2; Ts^2/2 Ts]
kf = KalmanFilter(A, B, C, D, R1, R2)
@@ -94,4 +94,4 @@
end
Yh = reduce(hcat, yh)
-plot([Y Yh'], lab=["Measurement" "Adaptive estimate"])
plot([es σs σwts], lab=["Prediction error" "Z-score" "\$σ_{wt}\$ multiplier"], layout=2, sp=[1 1 2])
This time, the prediction errors look more like white noise centered around zero after the initial transient caused by the transition.
This tutorial demonstrated simple Kalman filtering for a double integrator without control inputs. We saw how the filtering estimate could be improved by playing around with the covariance matrices of the estimator, helping it catch up to fast changes in the behavior of the system without sacrificing steady-state noise properties.
In this case, we handled the modification of $R_1$ outside of the filter, implementing our own filtering loop. Some applications get away with instead providing time-varying matrices in the form of a 3-dimension array, where the third dimension corresponds to time, or instead of providing a matrix, providing a function $R_1(x, u, p, t)$ allows the matrix to be a function of state, input, parameters and time. These options apply to all matrices in the filter, including the dynamics matrices, $A,B,C,D$.
Lastly, we mention the ability of the KalmanFilter to act like a recursive least-squares estimator, by setting the "forgetting factor $α>1$ when creating the KalmanFilter. $α>1$ will cause the filter will exhibit exponential forgetting similar to an RLS estimator, in addition to the covariance inflation due to R1. It is thus possible to get a RLS-like algorithm by setting $R_1 = 0, R_2 = 1/α$ and $α > 1$.
See this notebook for a blog post about disturbance modeling and noise tuning using LowLevelParticleFilter.jl
Settings
This document was generated with Documenter.jl version 1.8.0 on Saturday 21 December 2024. Using Julia version 1.11.2.