Tuning Q matrix for CV and CA models in Kalman Filter
Intro
Kalman Filter (KF) is widely used for vehicle navigation tasks, and in particular for vehicle trajectory smoothing. One of the problems associated while applying the KF for navigation tasks is the modeling of the vehicle trajectory. For simplicity, it is convenient to choose a Constant Velocity (CV) model or a Constant Acceleration (CA) model for a wide range of tracking problems, where the position derivative is indeed the velocity and the velocity is (nearly) constant (for CV model). This advantage provides keeping dealing with a linear and stable system, as one demand from this type of tracking problem. Aside from Q tuning, there are additional parameters the designer should pre-define in the KF. The measurement noise covariance, R, initial error covariance, and the step size. The most important matrices in the KF are Q and R. The matrix R can be determined directly from the sensors, while this is not the case for Q.
Formalization
Consider two models, the CV model with position (p) and velocity (v) states, and the CA model with a position, velocity, and acceleration (a). The trajectory (system) matrix in the state-space is given by
The state-space itself is provided by
where w is a white Gaussian discretized noise. The Kalman filter algorithm is provided (for more details, please refer to Wiki):
Discussion
We (the filter designers) generally assume their system’s behavior is according to the trajectory model, where the velocity is indeed the position derivative, and the acceleration is indeed the velocity derivative. However, in real-life scenarios, we deal with non-Gaussian noise, discretization error, numerical considerations, measurement noise, nonlinearity, and many more. Hence, the correct way to establish a trajectory (system) model for using the Kalman filter is by considering the physical relations, but, add some white noise to handle the real-life scenarios. This noise is injected into the model (more specifically, into the Kalman filter) to compensate the designer's uncertainty for his model. For example, we may assume the CV model wherein real-life the velocity changes rapidly. Then, an appropriate Q matrix should be defined to handle this scenario. It is important to mention that choosing high values for the Q matrix might result in filter divergence, as the designer has huge uncertainty for his modeling. So, there is an optimal matrix Q for each scenario- which is not trivial to find.
Approach 1- Trial and error
Trial and error. Starting by large value and reducing it until convergence. This might work in an offline setting, where the designer must remember to avoid overfitting.
Approach 2 -Innovation based adaptive method
The most common adaptive approach to tune Q in real-time is to consider the innovation quantity of the Kalman filter.
Approach 3- Generative
A naive approach to tune filter parameters requires the storage of the entire parameter vector. If it is possible, sometimes the following will work:
Approach 4- Scaling factor
A scaling factor is a great approach when R is known. The core idea is the relationship between Q and R through the innovation quantity. It is given by:
Summary
There are many approaches to tune Q in the Klaman filter. We summarized 4 common techniques to handle this challenge.
About the Author
Dr. Barak Or is a well-versed professional in the field of artificial intelligence and data fusion. He is a researcher, lecturer, and entrepreneur who has published numerous patents and articles in professional journals. Dr. Or is also the founder of ALMA Tech. LTD, an AI and advanced navigation company. He has worked with Qualcomm as DSP and machine learning algorithms expert. He completed his Ph.D. in machine learning for sensor fusion at the University of Haifa, Israel. He holds M.Sc. (2018) and B.Sc. (2016) degrees in Aerospace Engineering and B.A. in Economics and Management (2016, Cum Laude) from the Technion, Israel Institute of Technology. He has received several prizes and research grants from the Israel Innovation Authority, the Israeli Ministry of Defence, and the Israeli Ministry of Economic and Industrial. In 2021, he was nominated by the Technion for “graduate achievements” in the field of High-tech.
Website www.barakor.com Linkedin www.linkedin.com/in/barakor/ YouTube www.youtube.com/channel/UCYDidZ8GUzUy_tYtxvVjRiQ
[1] L. Zhang, D. Sidoti, A. Bienkowski, K. R. Pattipati, Y. Bar-Shalom, and D. L. Kleinman, |On the identification of noise covariances and adaptive Kalman filtering: A new look at a 50-year-old problem,” IEEE Access, vol. 8, pp. 59362-59388, 2020.
[2] P. Abbeel, A. Coates, M. Montemerlo, A. Y. Ng, and S. Thrun, “Discriminative training of Kalman filters.” in Robotics: Science and systems, vol. 2, 2005, p. 1.
[3] R. Mehra, “On the identification of variances and adaptive Kalman filtering”, IEEE Transactions on automatic control, vol. 15, no. 2, pp. 175{184, 1970.
[4] A. Mohamed and K. Schwarz, ”Adaptive Kalman filtering for INS/GPS,” Journal of Geodesy, vol. 73, no. 4, pp. 193{203, 1999.