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): ...

Here I want to demonstrate how the proposed nonlinear estimation algorithm solves an example estimation problem. Model formulation As a dynamic system for variable $y$ I consider a nonlinear damped oscillator with an external force $a$: $$ \ddot{y} + 2 \eta \omega \dot{y} (1 + \xi \dot{y}^2) + \omega^2 \sin y = a $$ The difference from the linear model are: The returning force is changed from $y$ to $\sin y$ – this can be viewed as abandoning the small angle approximation for a gravity pendulum The friction force now nonlinear with an additional factor of $1 + \xi \dot{y}^2$ – the friction increases for high speed Introducing the variables $$ x_1 \coloneqq y \\ x_2 \coloneqq \dot{y} \\ $$ we rewrite it as a first order system $$ \dot{x_1} = x_2 \\ \dot{x_2} = -\omega^2 \sin x_1 - 2 \eta \omega x_2 (1 + \xi x_2^2) + a $$ Introduce discrete time variables $$ x_k \coloneqq x(k \tau) \\ a_k \coloneqq a(k \tau) $$ Applying the first-order integration method we get the following discrete time equation: $$ x_{k + 1} = f(x_k, a_k) \\ \text{with }f(x, a) = \begin{bmatrix} x_1 + \tau x_2 \\ x_2 - \tau (\omega^2 \sin x_1 + 2 \eta \omega x_2 (1 + \xi x_2^2) + a) \end{bmatrix} $$ Let’s introduce the noise sources into it: ...

In this note I present a development of nonlinear batch estimation algorithm. Model description The state estimation problem in a nonlinear system is considered. The formulations is analogous to the linear case, but with nonlinear transition and measurement equations. We use uppercase letters to denote variables participating in the nonlinear model. Time transition and measurement equations for which are $$ X_{k+1} = f_k(X_k, W_k) \\ Z_k = h_k(X_k) + V_k $$ All the other assumptions remain the same. The task is to estimate $X_k$ for epochs $k = 0, 1, \ldots, N$. This will be done by solving an optimization problem. ...

In the previous post I’ve derived formulas for a batch state estimation «Kalman smoother» algorithm. Here I want to provide a numerical verification of its correctness along with the classical Kalman filter algorithm. Monte Carlo method The invaluable method of verification of estimation algorithms is Monte Carlo simulation: Generate a ground truth sequence of states and measurements according to the system model. It assumes generating process and measurement noises as pseudorandom numbers Run the estimation algorithm and compute its estimation errors as $\Delta x_k = \hat{x}_k - x_k$, where $\hat{x}_k$ and $x_k$ are the estimated and true state respectively at epoch $k$ Repeat steps 1 and 2 many times and compute sample mean and covariance of the errors $\Delta x_k$ for each epoch $k$ The sample mean must be close to zero and the sample covariance must match the covariance estimated by the algorithm. Alternatively sample root mean squared errors (as $\sqrt{\operatorname{E} [\Delta x_k]_i^2}$) can be compared with standard deviations provided by the estimation algorithm. This will check mean and covariance correctness simultaneously and this is the approach I usually use. ...

In this post Kalman smoother formulas are derived as a solution to an optimization problem. Problem formulation We consider an estimation problem for a discrete time linear stochastic system: $$ \begin{gather*} x_{k+1} = F_k x_k + G_k w_k \\ z_k = H_k x_k + v_k \\ \operatorname{E} x_0 = x_0^- \\ \operatorname{E} (x - x_0^-) (x - x_0^-)^T = P_0^- \succ 0 \\ \operatorname{E} w_k = 0 \\ \operatorname{E} w_k w_k^T = Q_k \succ 0 \\ \operatorname{E} v_k = 0 \\ \operatorname{E} v_k v_k^T = R_k \succ 0 \\ \operatorname{E} w_i w_j^T = 0 \text{ for } i \neq j \\ \operatorname{E} v_i v_j^T = 0 \text{ for } i \neq j \\ \operatorname{E} w_i v_j^T = 0 \\ \end{gather*} $$ ...