Non-Linear System Definitions - Application Kalman Filter

Non Linear system Definitions

When building kalman filter we define the system using mathematical equations like system equation, Odinary differential equation, Partial diferential equation, state-space equation, etc.

I am collecting a set of these equations for easy access.

Open Research Question

Is there a meta-model domain specific language for specifing these models and the corresponsing filter

How to use Json-ld based DSL be used to reperesent systems and the filters

Single Rotatry Joint Robot Arm

Copied from [1]

Consider the following example: A robot arm is moved by a single rotary joint. Since the robot arm is affected by gravity, the torque acting on the rotary joint depends on the current angle. The robot arm is observed by some sensor that is capable of measuring the absolute orientation. Our goal is to estimate the angle of the rotary joint.

The system can be modeled by

\[ \theta_{k+1}=a_{k}(\theta_{k})+w_{k} \]

with system function

\[ a_{k}(\theta_{k})=\theta_{k}+\underbrace{c_{1}\sin (\theta_{k})}_{\rm gravity}+\underbrace{c_{2}}_{\rm velocity}, \] where \(\theta_k\) is the state and \(w_k\) is Wrapped Normal-distributed noise. The constants c1,c2 can be derived from a physical model of the system.

The measurement equation is given by

\[ \hat{z}_{k}=\theta_{k}+v_{k}, \] where \(z_k\) is the orientation measurement and \(v_k\) is WrappdNormal-distributed noise.

Another representation of same system [2]

\[ \begin{align*} \boldsymbol{\theta }_{k} = &\boldsymbol{\theta }_{k-1}+d_{1} \sin (\boldsymbol{\theta }_{k-1})+d_{2}+\eta _{k-1}, \tag{35}\\ \mathbf{y}_{k} = \begin{bmatrix}\sin \left(\theta_{k}\right) \\\cos \left(\theta_{k}\right)\end{bmatrix}+\vartheta_{k},\tag{36} \end{align*} \]

where d1 and d2 are constants.

The initial true and estimated states are taken as θ0=0 and θ~0|0=π , respectively, while the initial variance is taken as P0|0=2 . We assign d1=0.1 , d2=0.15 , Q=0.1 , and R=diag(0.2,0.2) . The simulation is performed for 200 time-steps and angular RMSEs are computed by implementing 1000 Monte-Carlo simulations.

• [1] G. Kurz, I. Gilitschenski and U. D. Hanebeck, “Recursive nonlinear filtering for angular data based on circular distributions,” 2013 American Control Conference, Washington, DC, USA, 2013, pp. 5439-5445, doi: 10.1109/ACC.2013.6580688.
• [2]

Problem 2

Copied from [2]

The second problem considered is a general multivariate nonlinear angular estimation problem [14]. In this problem, the state dynamics are of oscillatory nature, while the measurement equation is a monotone increasing function of arguments (e.g. a positive quadratic form or its positive square root). Similar, system models often appear in sonar-based bearing measurements and GPS (Global positioning system)-based information on the angle of arrival. This problem has been widely used in literature [11], [14], [44], [45] for validating the filtering performance. The state-space model of this problem can be written as [14],

\[ \begin{align*} \boldsymbol {\theta }_{k}=&|2 \cos (\boldsymbol {\theta }_{k-1})|+{\eta _{k-1}}, \tag{37}\\ \mathbf {y}_{k}=&\sqrt {(1+ \boldsymbol {\theta }_{k}^{T} \boldsymbol {\theta }_{k})} +\vartheta _{k}.\tag{38}\end{align*} \]

We consider a three-dimensional system (θk∈D3,yk∈D ) and assign the initial true and estimated states as θ0=[0,−π,π]T and θ~0|0=[−π,π,π/2]T , respectively, while initial error covariance is taken as P0|0=2In . The noise covariances are assigned as Q=diag([0.05,0.05,0.05]) and R=0.1 . The states are estimated for 200 time-steps and the results are evaluated by performing 1000 Monte-Carlo runs.

Uncertainty Propagation in Python

Example based on [1] for Cartesian to Polar Transformation . Below is the text

The simple example of transforming the coordinates of a point from the Cartesian to the Polar plane is considered. We assume that the mean and standard uncertainty of the cartesian coordinates \((x, y)\) are known a priori and the quanti- ties are declared as uncertain objects as depicted in Fig. 2. The radial and angular coordinates \((r, θ)\) are computed by applying the transformation equations \[r = \sqrt{x^2 + y^2}\] and \[\theta = arctan(y/x)\], respectively.

[1] Automatic Uncertainty Propagation Based on the Unscented Transform