Posts

In this post I propose a novel filter applicable to problems of a certain kind which I call “azimuth alignment”. By this I mean problems where some dynamic quantities are computed and measured in two different frames rotated by unknown azimuth angle. One practical example could be the problem of initial heading determination of inertial navigation system. We can implement INS integration in a local frame with unknown azimuth relative to NED frame and try to estimate it by matching computed position or velocity with GNSS measurements. I believe there exists no standard and fully satisfactory approach to solve such problems. The possible approaches are: ...

In this note a problem of ellipsoid fitting to a set of points in arbitrary number of dimensions is considered. The algorithm development was motivated by the task of magnetometer calibration and some example results for that are given at the end. Problem formulation A hyper-ellipsoid in $m$ dimensions is characterized by a center $c \in R^m$ and a symmetric positive-definite shape matrix $P \in R^{m \times m}$ which encodes length and direction of its axes. Coordinates of a point $x \in R^m$ lying on an ellipsoid satisfy the equation: $$ (x - c)^T P^{-1} (x - c) = 1 $$ ...

Gaussian mixture state representation seems like a powerful approach, which can work as a useful generalization of unimodal Gaussian representation. However, its coverage in literature is scarce and unsatisfactory, not to say confusing. It makes sense to start building understanding from a basic, but fundamental problem — estimation in linear systems. Here a linear filter which estimates parameters of Gaussian mixture probability density is rigorously developed. Conceptually it will be very similar to classical Kalman filter. ...

In a previous post I’ve considered an intuitive engineering approach of handling noisy control input signals in Extended Kalman Filters. Here I want to develop a more rigorous view to this approach. Problem formulation Let’s consider a linear state transition equation for $x_k$ of the form: $$ x_{k + 1} = F_k x_k + B_k u_k, $$ where $u_k$ is the deterministic sequence of control vectors. Initial distribution of $x$ is given as normal: $$ x_0 \sim \mathcal{N}(\hat{x}_0, P_0) $$ ...

Synthesis of strapdown Inertial Measurement Unit (IMU) readings is required for simulation of inertial navigation systems and related algorithms development. Here I want to describe the algorithm of IMU synthesis implemented in pyins, which is probably one of the most useful functions in the library. Problem formulation The task is to compute strapdown IMU readings from a time series of position and attitude. The following points define the algorithm requirements: Data is provided at known time points $t_k$, not necessary equispaced Position is given as a time series of latitude, longitude and altitude – $\varphi(t_k), \lambda(t_k), h(t_k)$ Attitude is given as a time series of roll, pitch and heading angles – $\gamma(t_k), \theta(t_k), \psi(t_k)$ The algorithm must account for Earth rotation and ellipticity and use realistic gravity model The algorithm must be able to compute rate and increment (integral) readings Preliminarily considerations We want to generate IMU from sampled trajectory points instead of some predefined continuous functions to have more flexibility and convenience. Figuring out and implementing appropriate continuous functions for position and attitude is difficult task on its own, which is better to be avoided. ...

Here I compare properties of the robust EKF update algorithm with a classical outlier rejection approach based on normalized innovation checks. Outlier rejection approach A well known and efficient approach in Kalman filtering is analyzing whether normalized measurement innovations are reasonable. The innovation $$ e = z - H x^- $$ has theoretical covariance $$ S = H P^- H^T + R $$ The quadratic form $e^T S^{-1} e$ is expected to have a $\chi$-squared distribution and its excessively large values indicate a possible outlier, which should be rejected. For scalar measurements it is equivalent to rejection measurements when the innovation is larger than (for example) 3$\sigma$ with $\sigma = \sqrt{S}$. ...

When viewed as an optimization problem the Extended Kalman Filter update step can be generalized to incorporate a robust loss function in order to protect against outlier measurements. In this note the initial theoretical development is given. Introducing a robust loss function In the standard EKF update cost function the measurement residuals appear squared, which is known to be not robust against outliers. We can alleviate this problem by wrapping the measurement residual part with a loss function $\rho$ as follows: $$ E(X) = \frac{1}{2} (X - X^-)^T (P^-)^{-1} (X - X^-) + \frac{1}{2} \rho\left( (h(X) - Z)^T R^{-1} (h(X) - Z) \right) $$ The function $\rho$ must be approximately linear for small arguments (to achieve optimality of Kalman update) and sublinear for large arguments (to be robust agains outlier measurements). Let’s state its properties formally: ...

The Extended Kalman Filter is classically built as an extension of the linear Kalman filter using system and measurement models linearization. In this regard the update step in EKF is naturally done in a single step. The iterated EKF aims to improve the linearization point by doing several update iterations and recomputing measurement Jacobian each time. It’s known to be connected with nonlinear optimization. In this note I want to derive possible EKF update strategies starting from the optimization viewpoint. ...

It’s generally known that the heading angle in Inertial Navigation Systems (INS) is observable when the system undergoes acceleration (including turns at nonzero velocity). However if we look at the “modified error model”, it may seem that merely significantly nonzero constant velocity is sufficient. Here I want to figure out this subject using formal observability criteria and then give it intuitive physical explanation. INS equations in 2 dimensions To investigate heading observability, a simplified version of INS for a 2-dimensional motion is fully sufficient. The state is described relative to the world frame $n$ by 5 variables: ...

This is a short note on my view how to best implement Kalman correction formulas in a programing language. Basic formulas A vector measurement is linearly related to a state vector with an additive noise: $$ z = H x + v $$ Where $z$ and $H$ are a known vector and matrix and the noise vector $v$ has a known covariance matrix $R$. Thus the measurement model is defined by 3 elements – $z, H, R$. ...