The definition of the system is the main point to design afterwards the observer algorithm. It is important to notice that the definition of this system is the same of the one used for the RIEKF 5, but is slightly different from the used MEKF 6.
The following equations define the states that are going to be estimated (this is the first equation defined in (3-4)):
̆ ̇ ̆ ((( ⃗⃗ )
) ( ⃗ ))
( ̇) ( ) ̆ (( ) ) ̆
⃗ ̇ ̇
It can be seen that it is desired to estimate the attitude in a quaternion form, the velocity in the w-Frame, the gyro biases and the scaling factor of the accelerometers sensor model. After defining the system equation as stated above, the output equation that show the measurements taken into account as an aid to improve the estimates is outlined as:
( )
‖ ̆ (( ⃗ )
) ̆ ‖
(4-4)
This equation is the same as the second equation in (3-4), the differences are that here the equations are decoupled and in the reference they are shown in a matrix form.
The value of ( ⃗⃗ )
( ⃗⃗ ) ( ) is used for all the algorithms for the
simulations with the true data trajectories. For the case of the real-time data, the direct measurement is used instead of rotating the nominal value with the corresponding estimated attitude and adding some noise as explained in 3.3.1.
Like the explanation in 3.4.1 introduce, the observer uses errors extracted from the measurements to correct the estimates of the states and obtain better results. In the case of the observer, these error equations are multiplied by some gain factors to give a certain amount of importance to the measurements. In the invariant observer, the following are the error equations utilized for the observer to correct the states with the measurements of the velocity and magnetic field. In this case, these errors are defined in an invariant manner using the theory in 2.7. The equations outline an invariant error definition:
⃗ ̃ ( ̃) ( ) ⃗ ( ⃗ ) ̆̃ ( ) ̆̃ ( ⃗ ) ̆̃ ‖ ̆ (( ⃗ ) ) ̆ ‖ ̆̃ (4-5)
The definition of the invariant error will be of paramount importance for the design of the observer as will be seen on the following lines. Previously, the rotational rate and specific acceleration vectors are displayed:
̆̃ ((( ⃗⃗ )
) ( ⃗ ̃)) ̆̃
̃ ̆̃ (( ) ) ̆̃
The variables expressed above are described in the following table:
Variable Description
⃗⃗ Invariant output error in velocity measurement, in w-Frame.
⃗⃗ Invariant output error in magnetic field measurement, in w-Frame.
Estimated rotational rate vector, in w-Frame. Estimated specific acceleration vector, in w-
Frame.
Table 4-1: Invariant variables of the observer’s correction term.
These terms are used in the observer correction step and are multiplied by the gains to obtain the most suitable possible response. It is recommended to notice that they are functions of the estimates and the measurements; as a consequence they are known variables used for the construction of the observer.
Another characteristic to take into account is that the two latter variables, in the way they are defined, show the type of trajectory the UAV is performing. For instance, the case where ̇ → and , denotes the aircraft is in level-flight.
As it is investigated before, the concept of invariance of these error equations allow the system correction terms to be decoupled into the following four different subsystems.
The longitudinal subsystem:
( ̃̇ ̃̇ ̃̇ ) ( ( ) ( ) ( ) ) ( ̃ ̃ ̃ ) (4-7)
The lateral subsystem:
( ̃̇ ̃̇ ̃̇ ) ( ( ) ( ) ( ) ) ( ̃ ̃ ̃ ) (4-8)
( ̃̇ ̇) (
( )
( ) ) ( ̃ )
(4-9)
The heading subsystem:
( ̃̇ ̃̇ ) ( ( ) ( ̃) ( ) ( ̃) ) ( ̃ ̃ ) ( ( ) ( ̃) ( ̃) ( ) ( ̃) ( ̃) ) ̃ (4-10)
It is important to highlight that the nomenclature on the decoupling of the system into the four different subsystems above is not the same that it is used on this thesis. Thus, with the purpose of clarifying this important part that is directly extracted from [3], the following table displays the changes in nomenclature that apply only for the equations (4-7) to (4-10).
Variable Description
̆ (⃗⃗ ) Vector representing the attitude error, in the quaternion form.
⃗⃗ Vector representing the velocity error. ⃗⃗ Vector representing the gyro bias error. Vector representing the accelerometer scale-
factor error.
Table 4-2: Invariant error system variables.
These variables are defined in chapter 4 – page 63 of [3] and denote the definition of the invariant error system to decouple the system in the four equations above. Without this structure the system could not be decoupled and the gains defined to give more importance in determined measurements would be very difficult to tune. The next table give displays some other important variables utilized in the equations above.
Variable Description
̆ Quaternion representing a frame rotation of the vector to be vertical.
( ̃)
Vector representing the Earth magnetic field, rotated by ̆ .
Positive constant that relates the ( ⃗⃗ ) with .
⃗⃗
̃ Vector representing the attitude error, rotated by ̆ .
⃗⃗ ̃ Vector representing the velocity error, rotated by ̆ .
⃗⃗ ̃ Vector representing the gyro bias error, rotated by ̆ .
The meaning of each of the parameters above is not of particular importance. It is shown to understand that the gains act in different parts of the observer. For further details, it is recommended to check the reference [3].
Regarding the subsystems outlined before, the final gains to change the influence, which each of the measurements have on the estimates, are displayed:
⃗ ⃗ ⃗ 〈( ⃗ ) ⃗ 〉 ⃗ ⃗ ⃗ ⃗ ⃗ ⃗ 〈( ⃗ ) ⃗ 〉 ⃗ 〈 ⃗ 〉 ⃗ (4-11)
These are the terms applied to the system equations to create the observer. That is the reason why it is said that the implementation of an observer is usually straightforward, but not the design of such an observer. A proper discussion of the gains and their effects on the estimation of the states will be done in the following subsection 4.3.
After all that, the observer equations are outlined with the following equations: ̆̃̇ ̆̃ ((( ⃗⃗ ) ) ( ⃗ ̃)) ( ⃗ ⃗ ) ̆̃ ( ‖ ̆̃ ‖ ) ̆̃ ( ̃̇) ( ) ̃ ̆̃ (( ) ) ̆̃ ( ⃗ ⃗ ) ⃗ ̃̇ ̆̃ ( ⃗ ⃗ ) ̆̃ ̃̇ ̃ ( ⃗ ⃗ ) (4-12)
It is easily seen how straightforward is to identify on each of the equations the correction part and the propagation part, which is the part illustrated on the system equations. The term added in the quaternion equation denoted as ( ‖ ̆̃ ‖ ) ̆̃ is a well-known trick to
maintain the norm of the quaternion equal to 1. The constant is set to 1, but this is not the unique option.