Tuning Q matrix for CV and CA models in Kalman Filter

Dr Barak Or
Towards Data Science
5 min readJan 26, 2021

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.

Image by Author

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

Trajectory matrix

The state-space itself is provided by

State-space

where w is a white Gaussian discretized noise. The Kalman filter algorithm is provided (for more details, please refer to Wiki):

Kalman filter equations

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.

Innovation based approach

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:

Generative approach

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:

Scaling

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.

--

--

Towards Data Science
Towards Data Science

Published in Towards Data Science

Your home for data science and AI. The world’s leading publication for data science, data analytics, data engineering, machine learning, and artificial intelligence professionals.

Dr Barak Or
Dr Barak Or

Written by Dr Barak Or

Google and Reichman Tech School. AI Entrepreneur, Researcher, and Lecturer

No responses yet