

In [1]:

    
import numpy as np
import matplotlib.pyplot as plt

import matplotlib.pyplot as plt
from matplotlib import rcParams
rcParams['legend.fontsize'] = 14
rcParams['axes.labelsize'] = 14
rcParams['axes.titlesize'] = 14
%matplotlib inline

Dynamics of INS errors

This example analyses the behaviour of errors of an Inertial Navigation System. The errors (as long as they small enough) are described by a system of linear ODEs. When the system matrix is constant (INS resting on bench, straight flight at a constant speed) the system can be solved in a closed formed, but even in this case it is a cumbersome procedure. So pyins provides a simple routine for numerical propagation of INS errors.

Generate Trajectory

We will use a straight and level flight as a trajectory. We are going to generate data for 48 hours to consider a long term behaviour of the errors.



In [2]:

    
from pyins import sim

Because we are not going to use IMU readings we set high sampling period.



In [3]:

    
dt = 1

We specify the trajectory with time series of latitudes and longitudes and corresponding time stamps. Here we need only to set start and end points. Note that heading, pitch and roll will be computed automatically (if not provided) assuming that the velocity is directed along the longitudinal axis. For simplicity we consider the flight in the East direction. This situation is largely similar to static conditions, but nevertheless more interesting as it contains heading to position errors coupling due to motion.



In [4]:

    
traj, gyro, accel = sim.from_position(dt, [30, 30], [0, 90], t=[0, 48 * 3600])

On the plots we see that the velocity as well as the heading are constant.



In [5]:

    
plt.figure(figsize=(13, 4))

plt.subplot(131)
plt.plot(traj.lon, traj.lat)
plt.ylabel("lat, deg", fontsize=16)
plt.xlabel("lon, deg", fontsize=16)

plt.subplot(132)
plt.plot(traj.index * dt / 3600, traj.VE, label='VE')
plt.plot(traj.index * dt / 3600, traj.VN, label='VN')
plt.xlabel("time, h")
plt.ylabel("m/s")
plt.legend(fontsize=16, loc='center right')

plt.subplot(133)
plt.plot(traj.index * dt / 3600, traj.h, label='heading')
plt.xlabel("time, h")
plt.ylabel("deg")
plt.legend(loc='best')

plt.tight_layout()

Eigenvalues of the system

In our case the system matrix is constant, so we can analyze its behaviour by finding the eigenvalues. Here we will use a private function which are not generally intended to use from a user side.



In [6]:

    
from pyins.filt import _error_model_matrices
from scipy.linalg import eigvals



In [7]:

    
F = _error_model_matrices(traj.iloc[:1])[0][0]



In [8]:

    
eigvals(F)









    Out[8]:





array([ -3.16101694e-19 +1.28110153e-03j,
        -3.16101694e-19 -1.28110153e-03j,
         2.20563988e-19 +1.19908250e-03j,
         2.20563988e-19 -1.19908250e-03j,
        -1.17764528e-21 +0.00000000e+00j,
         3.93370901e-21 +8.20190298e-05j,   3.93370901e-21 -8.20190298e-05j])

So we see that all eigenvalues come in complex conjugate pairs and in fact they are imaginary (the real parts are numerical errors). It means that all modes are bounded and oscillatory. The periods are: 81.7 minutes, 87.3 minutes and 21.3 hours. The first two are slightly perturbed so called Schuller period, which is nominally 84 minutes. The last one is related to Earth rotation, but since our vehicle is moving in East direction it completes the rotation in 21.3 hours.

Example of errors propagation without sensor errors

Here we illustrates the previous findings by computing the errors solution for some initial values.



In [9]:

    
from pyins.filt import propagate_errors
from pyins import earth



In [10]:

    
def plot_errors(dt, err, step=1000):
    plt.figure(figsize=(15, 10))
    
    plt.subplot(331)
    err = err.iloc[::step]
    
    t = err.index * dt / 3600
    
    plt.plot(t, err.lat, label='lat')
    plt.xlabel("time, h")
    plt.ylabel("m")
    plt.legend(loc='best')
    
    plt.subplot(334)
    plt.plot(t, err.lon, label='lon')
    plt.xlabel("time, h")
    plt.ylabel("m")
    plt.legend(loc='best')
    
    plt.subplot(332)
    plt.plot(t, err.VE, label='VE')
    plt.xlabel("time, h")
    plt.ylabel("m/s")
    plt.legend(loc='best')
    
    plt.subplot(335)
    plt.plot(t, err.VN, label='VN')
    plt.xlabel("time, h")
    plt.ylabel("m/s")
    plt.legend(loc='best')

    plt.subplot(333)
    plt.plot(t, err.h, label='heading')
    plt.xlabel("time, h")
    plt.ylabel("deg")
    plt.legend(loc='best')
    
    plt.subplot(336)
    plt.plot(t, err.p, label='pitch')
    plt.xlabel("time, h")
    plt.ylabel("deg")
    plt.legend(loc='best')
    
    plt.subplot(339)
    plt.plot(t, err.r, label='roll')
    plt.xlabel("time, h")
    plt.ylabel("deg")
    plt.legend(loc='best')
    
    plt.tight_layout()



In [11]:

    
err = propagate_errors(dt, traj, d_lat=500, d_lon=-300, d_VE=2, d_VN=-1, d_h=0.05, d_p=0.01, d_r=-0.02)



In [12]:

    
plot_errors(dt, err)

It's easy to compute responses to individual error sources and thus compute the full transition matrix. But it will be hard and cumbersome to illustrate such result.

A simpler way is to repeat computations with some initial errors disabled and compare the results with the full picture. By such approach the following conclusions can be easily verified:

An initial longitude error doesn't excite other errors and doesn't evolve itself. It means that it's impossible to distinguish between different values of longitude and the navigation can be started from any value.
An initial latitude error doesn't influence other errors significantly.
Velocity, pitch and roll errors form a subsystem weakly influenced by other errors.
Heading error is mostly determined by initial heading, pitch and roll errors.

Note that 2, 3, 4 are true as long as $\Delta r / R$ ($\Delta r$ is the position error and $R$ is Earth radius) is significantly less than attitude errors.

Errors due accelerometer biases

Now we investigate errors propagation when the inertial sensors containing constant biases.

We consider errors due to the East accelerometer bias (keep in mind that the attitude is constant and the heading is 90 degrees).



In [13]:

    
err = propagate_errors(dt, traj, d_accel=[0, 1e-2, 0])

We see that accelerometer biases also creates oscillatory errors. The things worth noting are that longitude, pitch and heading errors are biased relative to zero by their oscillation amplitudes.

Errors due to the North bias are conceptually the same, with East and North axes switched. The heading error won't have a bias.

Errors due to the vertical accelerometer bias is irrelevant for horizontal errors components.



In [14]:

    
plot_errors(dt, err)

Errors due to gyroscope biases

First consider errors due to the East gyro bias.



In [15]:

    
err = propagate_errors(dt, traj, d_gyro=[0, 1e-7, 0])

We see that errors are bounded, with longitude and heading errors biased by the value of their amplitudes.



In [16]:

    
plot_errors(dt, err)

Now consider errors due to the North bias.



In [17]:

    
err = propagate_errors(dt, traj, d_gyro=[-1e-7, 0, 0])  # North bias is +1e-7.

This is the first case when the error in longitude growth approximately linearly to an unbounded value. The rate of the longitude error growth is $R \epsilon_N \cos^2 \varphi$ , where $\varphi$ is the latitude and $\epsilon$ is the North gyro bias.

In our case the growth in kilometers per hour is equal to:



In [18]:

    
1e-7 * earth.R0 * 3.6 * np.cos(np.deg2rad(traj.lat[0])) ** 2









    Out[18]:





1.7220969900000003

So the expected final error is 1.72 * 48 = 82.5 km, which matches well with the simulation results.



In [19]:

    
plot_errors(dt, err)

Now consider errors due to the vertical bias.



In [20]:

    
err = propagate_errors(dt, traj, d_gyro=[0, 0, 1e-7])

In this case the longitude error also growths unbounded. The rate of growth is equal to $R \epsilon_N \sin \varphi \cos \varphi$:



In [21]:

    
1e-7 * earth.R0 * 3.6 * np.sin(np.deg2rad(traj.lat[0])) * np.cos(np.deg2rad(traj.lat[0]))









    Out[21]:





0.99425316074714409

And the expected final error is 0.99 * 48 = 47.5 km. The simulation value is slightly lower because of sinusoidal modulations.



In [22]:

    
plot_errors(dt, err)

Conclusion

When the coefficients of the error equations are constant, then:

The horizontal INS errors contain only oscillatory modes. The two periods are 24 hours (corrected by a vehicle East velocity) and the 84 minutes (slightly perturbed depending on a latitude and a vehicle's velocity).
The response to constant accelerometer biases is also bounded and oscillatory. The same holds true for a constant East gyro bias.
In a presence of North and vertical gyro biases the longitude error growths unbounded, with the rate of growth proportional to $R \varepsilon$. And this is the only unbounded error. It proves the fact that the pure inertial performance of an INS is determined by gyroscopes accuracy.

Generally the behaviour of INS errors can be analyzed at any level of detail and for arbitrary trajectories using pyins.



In [ ]: