An unscented Kalman filter for matrix Lie groups is proposed where the time propagation of the state is formulated on the Lie algebra. This is done with the kinematic differential equation of the logarithm, where the inverse of the right Jacobian is used. The sigma points can then be expressed as logarithms in vector form, and time propagation of the sigma points and the computation of the mean and the covariance can be done on the Lie algebra. The resulting formulation is to a large extent based on logarithms in vector form, and is therefore closer to the UKF for systems in $\mathbb{R}^n$. This gives an elegant and well-structured formulation which provides additional insight into the problem, and which is computationally efficient. The proposed method is in particular formulated and investigated on the matrix Lie group $SE(3)$. A discussion on right and left Jacobians is included, and a novel closed form solution for the inverse of the right Jacobian on $SE(3)$ is derived, which gives a compact representation involving fewer matrix operations. The proposed method is validated in simulations.
Virginia Polytechnic Institute and State University（弗吉尼亚理工学院暨州立大学）
In this work, we present an aided inertial navigation system for an autonomous underwater vehicle (AUV) using an unscented Kalman filter on manifolds (UKF-M). The inertial navigation estimate is aided by a Doppler velocity log (DVL), depth sensor, acoustic range and, while on the surface, GPS. The sensor model for each navigation sensor on the AUV is explicitly described, including compensation for lever arm offsets between the IMU and each sensor. Additionally, an outlier rejection step is proposed to reject measurement outliers that would degrade navigation performance. The UKF-M for AUV nav…
Delft University of Technology（代尔夫特理工大学）；University of Eindhoven（埃因霍温大学）
This paper presents a novel methodology to auto-tune an Unscented Kalman Filter (UKF). It involves using a Two-Stage Bayesian Optimisation (TSBO), based on a t-Student Process to optimise the process noise parameters of a UKF for vehicle sideslip angle estimation. Our method minimises performance metrics, given by the average sum of the states' and measurement' estimation error for various vehicle manoeuvres covering a wide range of vehicle behaviour. The predefined cost function is minimised through a TSBO which aims to find a location in the feasible region that maximises the probability of …
Korea Advanced Institute of Science and Technology（韩国科学技术院）
This paper proposes a simple, accurate and computationally efficient method to apply the ordinary unscented Kalman filter developed in Euclidean space to systems whose dynamics evolve on manifolds.We use the mathematical theory called stable embedding to make a variant of unscented Kalman filter that keeps state estimates in closeproximity to the manifold while exhibiting excellent estimation performance. We confirm the performance of our devised filter by applying it to the satellite system model and comparing the performance with other unscented Kalman filters devised specifically for system…
This paper describes a novel tracking filter, designed primarily for use in collision avoidance systems on autonomous surface vehicles (ASVs). The proposed methodology leverages real-time kinematic information broadcast via the Automatic Information System (AIS) messaging protocol, in order to estimate the position, speed, and heading of nearby cooperative targets. The state of each target is recursively estimated in geodetic coordinates using an unscented Kalman filter (UKF) with kinematic equations derived from the spherical law of cosines. This improves upon previous approaches, many of whi…
MINES ParisTech, PSL Research University（巴黎高等矿业学院，巴黎文理研究大学）；Safran Tech, Groupe Safran（赛峰科技，赛峰集团）
The present paper introduces a novel methodology for Unscented Kalman Filtering (UKF) on manifolds that extends previous work by the authors on UKF on Lie groups. Beyond filtering performance, the main interests of the approach are its versatility, as the method applies to numerous state estimation problems, and its simplicity of implementation for practitioners not being necessarily familiar with manifolds and Lie groups. We have developed the method on two independent open-source Python and Matlab frameworks we call UKF-M, for quickly implementing and testing the approach. The online reposit…
Fixed-Time Dynamic Landing of Quadrotors using Adaptive Unscented Kalman Filtering and Nonlinear Model Predictive Control
Autonomous Vehicles Laboratory, Department of Aerospace Engineering, Toronto Metropolitan University（自主车辆实验室，航空航天工程系，多伦多 Metropolitan 大学）；University of Toronto Institute for Aerospace Studies, University of Toronto（多伦多大学航空航天研究 institute，多伦多大学）
Unscented Orientation Estimation Based on the Bingham Distribution
Intelligent Sensor-Actuator-Systems Laboratory (ISAS) Institute for Anthropomatics Karlsruhe Institute of Technology (KIT), Germany（智能传感器-执行器系统实验室（ISAS） 人机学研究所 卡尔斯鲁厄大学（KIT） 德国）；Computer Graphics Group Department of Computer Science University College London (UCL), United Kingdom（计算机图形学组 计算机科学系 伦敦大学学院（UCL） 英国）
A Hybrid Neural-Assisted Unscented Kalman Filter for Unmanned Ground Vehicle Navigation
Autonomous Navigation and Sensor Fusion Lab（自主导航与传感器融合实验室）；Hatter Department of Marine Technologies（海洋技术系）；Charney School of Marine Sciences（海洋科学学院）；University of Haifa（海法大学）
Unified Orbit-Attitude Estimation and Sensor Tasking Framework for Autonomous Cislunar Space Domain Awareness Using Multiplicative Unscented Kalman Filter
Department of Mechanical and Aerospace Engineering（机械与航空航天工程系）；Missouri University of Science and Technology（密苏里科技大学）；Embry-Riddle Aeronautical University（艾姆伯-理德航空大学）
Chongqing University of Posts and Telecommunications（重庆邮电大学）；Swansea University（斯旺西大学）；Southwest University（西南大学）
Industrial robots play a vital role in automatic production, which have been widely utilized in industrial production activities, like handling and welding. However, due to an uncalibrated robot with machining tolerance and assembly tolerance, it suffers from low absolute positioning accuracy, which cannot satisfy the requirements of high-precision manufacture. To address this hot issue, we propose a novel calibration method based on an unscented Kalman filter and variable step-size Levenberg-Marquardt algorithm. This work has three ideas: a) proposing a novel variable step-size Levenberg-Marq…
The Kalman filter operates by storing a Gaussian description of the state estimate in the form of a mean and covariance. Instead of storing and manipulating the covariance matrix directly, a square-root Kalman filter only forms and updates a triangular matrix square root of the covariance matrix. The resulting algorithm is more numerically stable than a traditional Kalman filter, benefiting from double the working precision. This paper presents a formulation of the square root Kalman filter that leverages the QR decomposition to dramatically simplify the resulting algorithm.
Self-assessment is a key to safety and robustness in automated driving. In order to design safer and more robust automated driving functions, the goal is to self-assess the performance of each module in a whole automated driving system. One crucial component in automated driving systems is the tracking of surrounding objects, where the Kalman filter is the most fundamental tracking algorithm. For Kalman filters, some classical online consistency measures exist for self-assessment, which are based on classical probability theory. However, these classical approaches lack the ability to measure t…
Department of Navigation Engineering, Naval University of Engineering（海军工程大学导航工程系）
This note is devoted to deriving the measurement update of the geometric extended Kalman filter using the multiplicative extended Kalman filtering approach, resulting in the attitude estimator referred as geometric multiplicative extended Kalman filter. The equivalence of the derived geometric multiplicative extended Kalman filter and geometric extended Kalman filter is also demonstrated in this note.
TIER IV, Inc.（TIER IV公司）；Massachusetts Institute of Technology（麻省理工学院）；NASA Jet Propulsion Lab, California Institute of Technology（加州理工学院NASA喷气推进实验室）；University of Michigan（密歇根大学）；The University of Tokyo（东京大学）
This paper develops a new nonlinear filter, called Moment-based Kalman Filter (MKF), using the exact moment propagation method. Existing state estimation methods use linearization techniques or sampling points to compute approximate values of moments. However, moment propagation of probability distributions of random variables through nonlinear process and measurement models play a key role in the development of state estimation and directly affects their performance. The proposed moment propagation procedure can compute exact moments for non-Gaussian as well as non-independent Gaussian random…
Many state estimation and control algorithms require knowledge of how probability distributions propagate through dynamical systems. However, despite hybrid dynamical systems becoming increasingly important in many fields, there has been little work on utilizing the knowledge of how probability distributions map through hybrid transitions. Here, we make use of a propagation law that employs the saltation matrix (a first-order update to the sensitivity equation) to create the Salted Kalman Filter (SKF), a natural extension of the Kalman Filter and Extended Kalman Filter to hybrid dynamical syst…
Kalman filter is presumably one of the most important and extensively used filtering techniques in modern control systems. Yet, nearly all current variants of Kalman filters are formulated in the Euclidean space $\mathbb{R}^n$, while many real-world systems (e.g., robotic systems) are really evolving on manifolds. In this paper, we propose a method to develop Kalman filters for such on-manifold systems. Utilizing $\boxplus$, $\boxminus$ operations and further defining an oplus operation on the respective manifold, we propose a canonical representation of the on-manifold system. Such a canonica…
Indoor Position and Attitude Tracking with SO(3) Manifold
King Fahd University of Petroleum and Minerals（法赫德石油与矿产大学）；King Abdullah University of Science and Technology（阿卜杜拉国王科技大学）；aramco research center（阿美石油公司研究中心）
This study considers the object localization problem and proposes a novel multiparticle Kalman filter to solve it in complex and symmetric environments. Two well-known classes of filtering algorithms to solve the localization problem are Kalman filter-based methods and particle filter-based methods. We consider these classes, demonstrate their complementary properties, and propose a novel filtering algorithm that takes the best from two classes. We evaluate the multiparticle Kalman filter in symmetric and noisy environments. Such environments are especially challenging for both classes of clas…
