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