Saturday, October 11, 2014

A state estimation example, GPS aided Altimeter. Part three

Previous post here

In this post we will examine a one degree of freedom inertial unit aided by a GPS altitude measurement. Download from this links the example Matlab or Scilab script.
In the last two articles a basic inertial unit has been presented, in this post we will use measurements from INS and GPS to obtain an altitude sensor. 
Let's pretend GPS sample rate \(T\) is 1 second.  The new altitude measurement equation is
with   \(k\)  positive integer

Between two successive samples the system works exactly as per precedent article equations, the altitude error grows fast.
Every instant \(t=kT\) the fresh measurement arrives from GPS, then a error signal is calculated subtracting the INS position estimate from the GPS measurement. 
$$\delta y_k=\tilde{z}_k-\hat{z}_k=\boldsymbol{C}\delta\hat{x}_k +\nu_z(k) $$
The observability matrix is eye(3,3); The rank is three hence the system is completely observable.
State error evolution is described by the following discrete linear system
$$\boldsymbol{\Phi}=exp(\boldsymbol{A} T)$$
$$\delta \boldsymbol{\hat{x}}^-_{k+1}=\boldsymbol{\Phi}\delta\boldsymbol{\hat{x}}^+_k$$
We set \(var(\nu_z(kT))= 4 m^2\) and \(L=[0.4,0.04,0.002]'\)
Refer to script file for the other parameters values.

The altitude error behavior is plotted in figure 1, the response dynamic is due to the choice of L. 
Figure 1 Error Deviation Plot

With the use of both sensors the altitude deviation is bounded and its final value is near 1.2m; that value is nicely lower than GPS only deviation value that is 2m. To get a stable altitude value are necessary around 50 samples, with our sample rate it is equivalent to 50s. To shape the time response it is necessary, for example, to fine tune the state observer behavior with the poles placement technique.

The filter layout of figure 2 includes an observer and measurements from both the sensors,  In figure 3 you find the digital implementation that includes the INS model; a zero order realization is assumed.

Figure 2 Filter layout
Figure 3 Filter, full digital implementation T is the discrete INS sample time

The shown discretization of INS fits well the system. In figure 4 the response of continuous system is compared with analytical zero order hold discretization and numerical Tustin discretization.
Figure 4 INS only model Input Vs Output and magnified detail view

The figure 2 layout can be used with different kind of sensors;for example,but not only, barometric altimeters, sonar range finders and LIDAR. If the filter is used for altitude measurement the acceleration component along gravity vector and GPS altitude should be used as the system inputs.

The provided zero order hold implementation have a low computational cost and can be implemented on mobile devices. The overall error variance has been bound and its value is lower than that of single instruments.

To the following part
Post a Comment