## Thursday, July 31, 2014

### A state estimation example: GPS-aided altimeter

The value of telemetry data can be enhanced by the application of basic estimation and modeling techniques. In most real-life cases, even in the simplest systems, it is either too expensive or actually impossible to measure every relevant variable. Thus, one needs to make do with a reduced number of sensors, which are also quite noisy.

In the next few articles, I will present a complete application example in the form of a miniseries. The core idea is to investigate the performance of an algorithm that uses data from accelerometers and a GPS to enhance altitude measurement. That method should be implementable on mobile devices and can enhance the previously implemented altimeter. The very same algorithm can be used to post-process basic flight telemetry data.

Miniseries summary:
• Introduction post: the state estimation problem and state observer
• Altitude measurement enhancement with accelerometer, performance evaluation
• Altitude measurement enhancement with accelerometer and GPS data, performance evaluation

Working code will be posted where needed.
In order to use telemetry data in a meaningful way, it is necessary to have a mathematical model of our physical system. One possible representation is the state space representation.
A continuous linear system is represented as
$$\boldsymbol{\dot{x}}(t)=\boldsymbol{A}(t)\boldsymbol{x}(t)+\boldsymbol{B(t)}\boldsymbol{u}(t)$$
$$\boldsymbol{y}(t)= \boldsymbol{C}(t)\boldsymbol{x}(t)+ \boldsymbol{D}(t)\boldsymbol{u}(t)$$
Bold notation indicates vectors or matrices. All quantities are time dependent. This is a fully deterministic model, whose the mathematical representation is a set of ordinary differential equations.
$$\boldsymbol{x}(t)$$ is the vector containing the physical system state variables, $$\boldsymbol{y}(t)$$ represents the output variables and $$\boldsymbol{u}(t)$$ is the system input. All the other matrices describe the relationships between those variables.
However, in our example case, we can make some assumptions towards the simplification of the system:
$$\boldsymbol{\dot{x}}(t)= \boldsymbol{A}\boldsymbol{x}(t)+ \boldsymbol{B}\boldsymbol{u}(t)$$
$$\boldsymbol{y}(t)= \boldsymbol{C}\boldsymbol{x}(t)+ \boldsymbol{D}\boldsymbol{u}(t)$$
Now system matrices are time independent. Last set of equations is a model for a Linear Time Invariant system. If $$\boldsymbol{x}$$ has a dimension of $$n \times 1$$, then we say that the system has a dimension of $$n$$.
To better explain the state space representation I'll use a simple mechanical system. Suppose we need the mathematical model of a falling apple.

Figure 1 Falling Apple

To simplify the math, suppose the falling apple is not rotating. Assume also that the apple is moving through the void, thus aerodynamics are not involved. The apple will fall vertically.

Figure 2 Model definition

The system dynamics can be described using Newton's second law:
$$\boldsymbol{F}=m\boldsymbol{a_z}= m\boldsymbol{g} \rightarrow a_z=g$$
Using standard notation we can write:
$$\begin{bmatrix}\dot{p} \\ \dot{v} \end{bmatrix} =\begin{bmatrix}0 &1 \\ 0& 0 \end{bmatrix} \begin{bmatrix}p \\ v \end{bmatrix}+ \begin{bmatrix}0 \\ 1 \end{bmatrix} g$$
Note that we have chosen not to model the output $$\boldsymbol{y}(t)$$. It is not necessary in this example.
You can observe that such a model is not completely realistic. According to it, the apple will fall to infinity with a uniformly accelerated motion. As in most occasions the model is accurate only around specific conditions: the previous apple model is accurate for a short fall in the void, before the apple hits the plate. Commonly the model of a phenomenon is not linear but can be considered linear around predefined state variables values. That's one reason why linear models are useful for modeling real-world systems.

Figure 3 Mass-Spring-Damper System

Let’s move over to a more complex system and examine a complete numerical example. The system is composed by a mass, a spring and a damper. It is a classic literature example, for which you can find a description in this link.

Let's suppose that we want to know the position and the speed of the mass with a single position sensor. I know it's simple to find the solution, however the systematic solution that I will show here can be used on more complex systems. in particular it will be used on the inertial navigation system which will be presented later.

State estimation consists of getting the values of state variables without measuring them. We will achieve such a result with the use of a state observer.

The main working hypothesis is that the physical system is correctly modeled, in this case as an LTI system  of known $$\boldsymbol{A}, \boldsymbol{B}, \boldsymbol{C}, \boldsymbol{D}$$ matrices. The model of the system is supposed to describe exactly the physical system behavior. We do not need to identify the parameters or the order our system but we are only looking for the state space variables values. That the main idea is to get measurement values from a limited set of sensors and obtain the observed state estimation. To that goal, we will use a Luenberger Observer architecture, an estimator which is a closed loop state observer.

Figure 4 Estimation layout

Estimated variables will be indicated with the circumflex or hat notation.
Returning to the mass-spring-damper system the continuous time estimator will be described by the following system of equations. The only available sensor measurement  is the mass position. The estimator gain is contained in the $$\boldsymbol{L}$$ matrix.
$$\boldsymbol{\dot{\hat{x}}}(t)=\boldsymbol{A}\boldsymbol{\hat{x}}(t)+\boldsymbol{B}\boldsymbol{u}(t)+\boldsymbol{L}(\boldsymbol{y}(t)-\boldsymbol{\hat{y}}(t))$$
$$\boldsymbol{\hat{y}}(t)=\boldsymbol{C}\boldsymbol{\hat{x}}(t)+ \boldsymbol{D}\boldsymbol{u}(t)$$
Given spring constant $$k$$, damping constant $$c$$ and body mass $$m$$, we get
$$\boldsymbol{A}=\begin{bmatrix}0&1 \\ -k/m&–c/m \end{bmatrix}$$
$$\boldsymbol{B}=\begin{bmatrix} 0 \\ 1/m \end{bmatrix}$$
$$\boldsymbol{C}=\begin{bmatrix} 1 & 0\end{bmatrix}$$
$$\boldsymbol{D}=\begin{bmatrix} 0\end{bmatrix}$$
For presentation purposes, in the provided script files, the simulation state starts from [1 ; 0] and the system input is set to zero. You can see how the position oscillates with time and how well the observers track the velocity state.

Figure 5 Time response of system and state observer

Prior to the use of an observer it's necessary to check the observability matrix rank. If the rank is equal to the system order then it is possible to observe every state of the system. For our system $$O=[C;C*A]$$ and the rank of $$O$$ is two, equal to the rank of our system $$n$$ which is also is two. Hence the system is completely observable (command rank(O)). Another property that should be checked is the system stability. For now we will avoid to deal with unstable systems. By issuing the command spec(A) in Scilab or eig(A) in Matlab we get the system eigenvalues, which are also called poles of the system. If the real part of every pole is negative then the system is stable. In our case the eigenvalues are a complex conjugate pair -0.1 ±0.53i, and the system is stable.
With complex eigenvalues the state variables trajectories will oscillate when moving from one stable solution to another. This behavior is evident in the plots of figure 5.

If we define the tracking error as $$\boldsymbol{e}(t)=\boldsymbol{\hat{x}}(t)-\boldsymbol{x}(t)$$ we get
$$\boldsymbol{\dot{e}}(t)=(\boldsymbol{A}-\boldsymbol{L}\boldsymbol{C})\boldsymbol{e}$$
Hence, the error dynamic response is directly defined by $$\boldsymbol{L}$$ matrix. The poles of $$\boldsymbol{A-LC}$$ should be stable to have a usable estimator. In our example we get the stable complex poles -0.6 ±2.1i, which generate an oscillating, but stable, trajectory that is evident in figure 5.

With the dynamic of the error so well defined, it is possible to shape the response, for example with a pole placement method.  If we want two real poles at -5, the we can issue the Scilab command Lt=(ppol(A',C',[-0.5 ;-0.5]))'. Afterwards we can check the closed loop poles values with spec(A-Lt*C). In Matlab the commands are Lt= place(A',C',p).' and eig(A-Lt*C).

After this little introduction we have the tools to understand the altitude measurement system enhanced with accelerometers and GPS. A key point to our further analysis will be the characterization of the error variance of the state estimation.