Posts

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$. ...

In this post (recommended to read beforehand) I’ve shown a derivation of the Kalman smoother as a solution to an optimization problem. The resulting formulas are surprisingly elegant, however their applicability depends on the assumption that apriori filter covariance matrices $P_k^-$ are positive definite and invertible. This assumption might be limiting in practical problems and thus another form of the Kalman smoother is derived here. Motivating example A singular covariance matrix may arise in the following practical scenario. Imagine that we want to process measurements which relate states at the current and previous epochs – $x_k$ and $x_{k-1}$. A possible example might be processing of distance increments from an odometeter in a navigation algorithm. Such measurements are not directly supported by Kalman filter or smoother algorithms. However we can cast them into the Kalman framework by considering an augmented vector $$ x^a_{k + 1} = \begin{bmatrix} x_{k + 1} \\ x_k \end{bmatrix} $$ The approach is also known as «stochastic cloning». ...

In this note I want to derive Inertial Navigation System (INS) error equations in concise and coherent fashion. This is of course a well known subject, however it’s useful to derive all the equations from scratch and revisit the subject from time to time. INS state variables and equations for them An Inertial Navigation System computes position, velocity and attitude of a moving object using gyro and accelerometer measurements from an Inertial Measurement Unit (IMU). ...

In this note I want to show that there are two possible scenarios how noisy input can influence state time propagation in estimation problems. This is not well articulated in classical literature and may create confusion when applying estimation algorithms to practical systems. I will illustrate the concept on a simple example. Consider a robot which can move in the direction of its longitudinal axis and rotate. Its state is described by 4 variables (2D case): ...