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

## Monday, July 14, 2014

### Pitot Correction for Position and Angular Rates

Pitot probes are widely used on DIY aircraft and have proven to be to be a robust and reliable method to measure airspeed. Their basic principles are explained in the following link.

Installation position and the angular rates of the carrying vehicle affect Pitot probe measurements. In this article, I will present some corrections that should be applied to the Pitot measurements to remove such disturbances.

Prior to problem definition, let me stress the fact that Pitot probes, at least their total pressure port, are designed to minimize the uncertainty caused by non-zero angle of attack or angle of sideslip.

Figure TP.1 A plane flying at high AOA. Notice the airboom vanes

Nevertheless, special design and calibration should be employed when the AOA is really high. Such an example is the specialized Kiel probe design.

To push measurement accuracy to the edge, an accurate calibration of the probe with respect to angle of attack and sideslip should be available. [1 page 37 table 1 and followings]

Let's consider a nose mounted Pitot probe, installed on the fuselage axis of symmetry at a distance $$d=0.5m$$ from the center of gravity. For the rest of this article, we will need to assume that there is no wind at the flying area.

The airspeed recorded by the Pitot probe is expressed in the wind frame of reference. Thus, it is not possible to use body velocity and “Pitot airspeed” in the same formulas. The body frame of reference origin lies on the COG, whereas the frame of reference of the Pitot airspeed has the same origin but is oriented at the wind direction. As a result, the wind frame of reference is rotated with respect to the body frame by the angle of attack and the angle of sideslip.

Figure TP.2 Body reference frame definition from BAD article

In the previous figure there is a visual depiction of the relative rotation between the body frame and the wind frame. Denoting the three body frame velocity components axis as $$u,v,w$$ and the airspeed as $$V$$ then
Equations TP.1
$$u=Vcos(\alpha)cos(\beta)$$
$$v=Vsin(\beta)$$
$$w=Vsin(\alpha)cos(\beta)$$
$$|V|=\sqrt{u^2+v^2+w^2}$$
If we need to acquire the body velocity for our computations we should convert airspeed to the body frame as per the previous formulas.
Let's consider a numeric example based on Eq.1. The aircraft is travelling at 100 km/h with $$\alpha=10°$$ and $$\beta=0°$$ in a level straight flight path. A planar trajectory is assumed: we're flying on a vertical plane. In the body frame the velocity expression is $$u=98.4 km/h$$ and $$w=17.5 km/h$$
The calculated body velocity should match with the velocity measured by an IMU.

Afterwards, we will investigate the impact of body rotation rates, which create an additional, unwanted velocity component on the tip of the Pitot probe. Since what we are interested in is the center of gravity velocity, the additional velocity should be compensated for. However, this error term does not manifest if the probe tip coincides with the center of gravity, but this would be very unusual.

Let us define the position of the probe tip in the body frame as $$\textbf{p}=\begin{bmatrix} d \\ 0 \\ 0 \end{bmatrix}$$

Angular rates in the body frame are $$\boldsymbol{\omega }=\begin{bmatrix} p_b \\ q_b \\ r_b \\ \end{bmatrix}$$
These are the roll rate, pitch rate and yaw rate.

The additional speed term at the probe tip is $$\dot{p_b}=\boldsymbol{\omega_b}\times \textbf{p}$$

To better understand this error term, let's assume and examine a planar rotation, $$\boldsymbol{\omega_b}=\begin{bmatrix} 0 \\ q_b \\ 0 \\ \end{bmatrix}$$.

We chose a high value for pitch rate $$q_b=3 s^{-1}$$ that corresponds to about 170 degrees of rotation every second. $$\dot{p}=\begin{bmatrix}0\\0\\–dq_b\end{bmatrix}= \boldsymbol{\omega}\times \textbf{p}$$. The body velocity at the probe tip now has a new component along $$y_b$$ axis with a magnitude of 1.5m/s.

While the body frame makes it easy to deal with vehicle forces and moments, the wind frame provides a better visualization of the impact of body rates on the Pitot probe measurements. However, the question arises: how are body angular rates and wind angular rates related to each other?

Figure TP.3 Problem sketch

Have a look at the figure TP.3 that illustrates the problem. The first thing we should do is convert the position vector of the probe tip from body to wind frame.

The coordinates in the wind frame are calculated with the use of a rotation matrix or direction cosine matrix (rotation by angle of attack, then rotation by angle of sideslip)
$$R_b^w=\begin{bmatrix}cos(\alpha)cos(\beta) & sin(\beta) & sin(\alpha)cos(\beta) \\-cos(\alpha)sin(\beta) & cos(\beta) & –sin(\alpha)sin(\beta) \\ -sin(\alpha) & 0 & cos(\alpha) \end{bmatrix}$$
$$\textbf{p}_w=R_b^w \textbf{p}_b$$
By substitution
$$R_{wb}=\begin{bmatrix} 0.984&0&0.173 \\ 0&1&0 \\-0.173&0&0.984 \end{bmatrix}$$ $$\textbf{p}_w=\begin{bmatrix}0.984d \\ 0 \\ -0.173d \end{bmatrix}$$
Note that the pitot tip distance from cog still the, as expected, equal to d $$d_w=\sqrt{0.984^2d^2+0.173^2d^2}=d$$.

The following equation gives the explicit relation between the two angular rates.
$$\begin{bmatrix} p_w \\ q_w \\ r_w \end{bmatrix}= R_b^w\begin{bmatrix} p_b-\dot{\beta}sin(\alpha) \\ q_b-\dot{\alpha} \\ r_b+\dot{\beta}cos(\alpha) \end{bmatrix}$$
Given $$\alpha=10°,\beta=0°,\dot(\alpha),\dot{\beta}=0,p_b=0,q_b=3s^{-1},r_b=0$$
$$\boldsymbol{\omega}_w=\begin{bmatrix} 0 \\ q_w=q_b \\ r_w \end{bmatrix}= \begin{bmatrix} 0.984&0&0.173 \\0&1&0 \\-0.173&0&0.984 \end{bmatrix} \begin{bmatrix} 0 \\ q_b \\0 \end{bmatrix}$$

For this particular case the wind angular rates are identical to the body angular rates. Wind angular rates are correlated also with $$\dot{\alpha}$$ and $$\dot{\beta}$$. Should $$\beta$$ be non-zero then all the angular rates are coupled with each other.

$$\dot{\textbf{p}_w}=\boldsymbol{\omega}_w\times \textbf{p}_w=\begin{bmatrix}-0.259\\0\\-1.47 \end{bmatrix}$$
The probe is measuring the true airspeed $$\textbf{V}=\begin{bmatrix}27.7=100km/h\\0\\0 \end{bmatrix}$$ plus the speed induced by the rotation.
The resulting airspeed is $$\textbf{V}_m=\textbf{V}+\dot{\textbf{p}_w}$$ or $$\begin{bmatrix}27.4 \\0\\-1.47 \end{bmatrix}$$ which yields $$|\textbf{V}_m|=27.43 m/s$$, hence the error is 1.1%

Neglecting angular rates will introduce an uncertainty in the airspeed measurements and we have evaluated the magnitude of the introduced uncertainty to some degree.

Using the previous formulas, airspeed deviation can be calculated and compensated for, for a given flight condition and for any probe installation position. The overall compensation accuracy should be examined in detail as it's based on the knowledge of the position of the center of gravity, $$\alpha$$,$$\beta$$ and their derivatives.

In the majority of applications, this compensation is not done due to lack of accurate air data. This analysis can be used to complement the selection of probe installation position.

## Thursday, July 3, 2014

### Coordinate System

When referring to aircraft position, velocity, acceleration, orientation and angular velocity, the coordinate system in which they are expressed should always be mentioned. While frames of reference are arbitrary and everyone can use a different one to get the same results, it is convenient to use a common one. This makes it easy to share calculation procedures and data and compare them directly. Also, let's not neglect the fact that some coordinate systems are more intuitive to work with and are more suitable for specific work cases.
A few options are available, when it comes to choosing a coordinate system for aircraft or airborne systems in general. We will use the North-East-Down system (NED), due to its popularity and ease of use . In the following paragraphs, the NED frame will be defined and explained.

As the Earth is moving and rotating, we need a reference frame that is attached to the Earth itself. This is mostly imposed by the common characteristics of flight missions, which are expressed in terms of ground features (eg surface waypoints and landing sites). A common right-handed coordinate system is the Earth-Centered, Earth-Fixed frame (ECEF). Its origin is the center of the Earth. The X-axis points to the intersection between the Equator and the Prime meridian (latitude 0°, longitude 0°), the Y-axis points to 0° latitude and 90° longitude and the Z-axis points to the North Pole (latitude 90° along the axis of rotation of the Earth).
Figure CS 1 ECEF Coordinates

To use this coordinate system, the shape of the surface of the Earth should be analytically defined and a widespread definition is the WGS-84.
At this point, we will make two working hypotheses. First the curvature of the Earth will be neglected. This is a valid hypothesis when the airborne vehicle operates over a small area. The second notable assumption is that the atmosphere (the air mass over the Earth) is not moving in relation to the surface.

Next, we need a local reference frame or a local geographic frame. That frame is necessary in order to describe local trajectories and altitude. We can use local coordinates because the proposed reference system is used under the flat Earth hypothesis. This assumption will not hold when the vehicle travels far away from its origin, in which case the system equations will not describe reality adequately.
To this goal, a plane tangent to the surface of the Earth is defined and is called Local Tangent Plane (LTP). To describe quantities in relation to the LTP, we will use the North-East-Down (NED) convention. In this convention, the "North" axis points North in the local meridian direction and the "East" axis points East in the local parallel direction. These directions span a Cartesian plane on the LTP. The final, "Down" axis is perpendicular to the other two axes and points towards the Earth, to complete a right-handed coordinate system. Note that the "Down" axis doesn't point to the center of the Earth, but is defined by the other two axes and its direction depends on the latitude and longitude of the origin of the NED frame. Refer to the following figure for a graphical definition.
Figure CS 2 NED frame

The origin of the NED frame is fixed in ECEF coordinates. It can be chosen arbitrarily at a point on the surface at the operational site of the aircraft. and moves with the aircraft.

Next, we introduce the "body" frame of reference that has its origin coincident with the center of gravity of the aircraft, thus it moves with the aircraft itself. Refer to the following figure. The X-axis of the body frame is pointed toward the aircraft nose, the Y-axis is along the right wing and Z-axis points down. In the case where the aircraft is not rotated, the body axes have the same orientation as NED axes.

Figure CS 3 body frame

Rotation along body X-axis is called $$\phi$$ or roll, rotation along Y-axis is called $$\theta$$ or pitch and rotation along Z-axis is called $$\psi$$ or yaw. The set of those three rotations (roll, pitch, yaw) is called “Euler angles” and describes the orientation of the aircraft in relation to the NED frame. The body frame can be aligned with the NED frame through the Euler angles in a specific sequence. Starting from the NED frame, we perform a rotation around NED Z-axis by the yaw angle. Next, we rotate this new frame by its Y-axis by the pitch angle. Finally, we rotate again by the new X-axis by the roll angle. The resulting frame is the body frame.

The body frame might be useful for a multitude of applications, but it is not convenient to work with when it comes to calculating aerodynamic forces and moments. To handle aerodynamics we define a "wind" frame that is pointed directly into the relative wind direction, as depicted in the following picture. The origin of the wind frame is coincident with body frame origin and its orientation is defined by the angle of attack $$\alpha$$ and the angle of sideslip $$\beta$$. Starting from the body frame, a down-positive rotation about the body Y-axis by the angle $$\alpha$$ aligns the body Z-axis with the wind Z-axis. Afterwards, a right-positive rotation about the Z-axis of this new, intermediate frame by the angle $$\beta$$ aligns the axes with the wind frame.

Figure CS 4 Wind frame