from pyins import sim

"""
Explanation: 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.
End of explanation
"""


dt = 1

"""
Explanation: Because we are not going to use IMU readings we set high sampling period.
End of explanation
"""


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

"""
Explanation: 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.
End of explanation
"""


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()

"""
Explanation: On the plots we see that the velocity as well as the heading are constant.
End of explanation
"""


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

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

eigvals(F)

"""
Explanation: 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.
End of explanation
"""


from pyins.filt import propagate_errors
from pyins import earth

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()

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)

plot_errors(dt, err)

"""
Explanation: 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.
End of explanation
"""


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

"""
Explanation: 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).
End of explanation
"""


plot_errors(dt, err)

"""
Explanation: 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.
End of explanation
"""


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

"""
Explanation: Errors due to gyroscope biases
First consider errors due to the East gyro bias.
End of explanation
"""


plot_errors(dt, err)

"""
Explanation: We see that errors are bounded, with longitude and heading errors biased by the value of their amplitudes.
End of explanation
"""


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

"""
Explanation: Now consider errors due to the North bias.
End of explanation
"""


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

"""
Explanation: 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:
End of explanation
"""


plot_errors(dt, err)

"""
Explanation: So the expected final error is 1.72 * 48 = 82.5 km, which matches well with the simulation results.
End of explanation
"""


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

"""
Explanation: Now consider errors due to the vertical bias.
End of explanation
"""


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

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


plot_errors(dt, err)

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


