<SYSTEM_TASK:>
Given the following text description, write Python code to implement the functionality described below step by step
<END_TASK>
<USER_TASK:>
Description:
 INS-GPS Integration
This notebook shows an idealized example of loose INS-GPS integration.
Creating a trajectory and generating inertial readings
First we need to generate a trajectory. To keep things simple we generate sort of a random walk trajectory by summing random displacements.
Step1:  Assume that each step is done in 10 seconds and define time stamps
Step2:  Add displacements to initial latitude and longitude
Step3:  We also append 20 minutes of INS being at rest
Step4:  Set pitch and roll angles to zeros
Step5:  Sensor sampling period is set to 0.1
Step6:  Run the simulation routine which will interpolate the trajectory and generate inertial readings
Step7:  The final trajectory is drawn below, the initial point is marked with a cross.
Step8:  Integrating ideal data
Just to check that everything is correct we want to integrate the generated gyro and accel readings.
Step9:  First we apply coning and sculling corrections
Step10:  And the run the integration.
Step11:  Compute integration error using a convenience function
Step12:  We see that attitude and velocity errors are vanishingly small. The position errors are less than 3 meters during 3 hours of operations, which is completely negligible compared to errors of even the most accurate INS.
Step13:  Integrating "real" data
Now we will run the navigation using inertial sensors with errors.
The error will be a sum of a random bias and additive white noise. We define magnitudes typical for moderately accurate navigation grade sensors.
Step14:  Compute biases as a random constants. To avoid a "bad case" in this example we generated biases uniformly within $[-2 \sigma, 2 \sigma]$.
Step15:  Now we apply errors to inertial readings
Step16:  Compute coning and sculling corrections
Step17:  An INS operation have to start with the self alignment. We devote 15 minutes of the initial rest for it
Step18:  Split the readings into alignment and navigation parts
Step19:  Compare estimated attitude angles with the true angles.
Step20:  Assume that the initial position is known with the accuracy typical to GPS receivers
Step21:  Assume that it is known that the navigation starts at rest ans set initial velocities to 0
Step22:  Now we can run the integration
Step23:  We see that even with very accurate gyros pure INS performance is not that good.
Step24:  Aiding from GPS
Now we will use idealize GPS position observations for INS errors estimation and correction.
We assume that GPS is available every second and use known exact values of latitude and longitude
Step25:  We will use an idealized model that GPS observations contain only additive normal errors with a standard deviation of 10 meters (note that in reality errors in outputs from GPS receivers behave much worse).
Step26:  To use GPS measurements in a navigation Kalman filter we wrap this data into a special object
Step27:  Also define gyro and accelerometer models using parameters defined above
Step28:  Now we can run a navigation Kalman filter which will blend INS and GPS data. In this example INS errors didn't grow very large, thus we can use a feedforward filter.
Step29:  We create a filter by passing sampling period and computed trajectory. To initialize the covariance matrix we pass standard deviations of the initial errors.
Currently the covariance matrix is initialized as diagonal, even though it can be done more rigorously, i.e consider correlations between sensor biases and attitude errors. But my view is that a reliable filter should not depend on such fine details, otherwise it is likely to fail in real conditions. So for the sake of simplicity it is implemented like this for now (can be changed later).
Theoretical attitude accuracy (sd values) from static gyrocompassing in our case is
Step30:  We run the filter and pass available measurements to it. The return value is the INS trajectory corrected by estimated errors.
Step31:  Now we want to investigate errors in the filtered trajectory.
Step32:  Obviously performance in terms of position and velocity accuracy is very good, but this is sort of expected because GPS provides coordinates directly. 
Attitude angle errors are generally decreasing as well, but the picture is less clear. We want to plot their standard deviation bounds estimated by the filter as well.
Step33:  The return value of FeedforwardFilter contains attributes err, sd, gyro_err, gyro_sd, accel_err, accel_sd for estimated trajectory errors and inertial sensor states and their standard deviations. Below we plot true errors for heading, pitch and roll with their 1-sigma bounds provided by the filter.
Generally we see that the filter's performance is adequate. It can be measured more precisely by Monte-Carlo simulation, but this will not be included in this example.
Step34:  Also it is interesting to assess the filter's sensor bias estimation. Plots below show $\pm \sigma$ bands of gyro bias estimates, the straight line depicts the true value. We see that estimation of gyro biases is quite successful.
Step35:  Below the same done for accelerometer biases. Horizontal accelerometer biases are less observable on the given trajectory than gyro biases, and the vertical bias is not observable at all because pitch and roll are held zero.
<ASSISTANT_TASK:>
Python Code:
from pyins import sim
from pyins.coord import perturb_ll
def generate_trajectory(n_points, min_step, max_step, angle_spread, random_state=0):
    rng = np.random.RandomState(random_state)
    
    xy = [np.zeros(2)]
    angle = rng.uniform(2 * np.pi)
    heading = [90 - angle]
    angle_spread = np.deg2rad(angle_spread)
    for i in range(n_points - 1):
        step = rng.uniform(min_step, max_step)
        xy.append(xy[-1] + step * np.array([np.cos(angle), np.sin(angle)]))
        angle += rng.uniform(-angle_spread, angle_spread)
        heading.append(90 - angle)
    
    return np.asarray(xy), np.asarray(heading)
xy, h = generate_trajectory(1000, 70, 100, 20, random_state=1)
Explanation: INS-GPS Integration
This notebook shows an idealized example of loose INS-GPS integration.
Creating a trajectory and generating inertial readings
First we need to generate a trajectory. To keep things simple we generate sort of a random walk trajectory by summing random displacements.
End of explanation
t = np.arange(1000) * 10
Explanation: Assume that each step is done in 10 seconds and define time stamps:
End of explanation
lat0 = 58
lon0 = 56
lat, lon = perturb_ll(lat0, lon0, xy[:, 1], xy[:, 0])
Explanation: Add displacements to initial latitude and longitude:
End of explanation
t = np.hstack((-1200, t))
lat = np.hstack((lat[0], lat))
lon = np.hstack((lon[0], lon))
h = np.hstack((h[0], h))
Explanation: We also append 20 minutes of INS being at rest:
End of explanation
p = np.zeros_like(h)
r = np.zeros_like(h)
Explanation: Set pitch and roll angles to zeros:
End of explanation
dt = 0.1
Explanation: Sensor sampling period is set to 0.1:
End of explanation
traj_ref, gyro, accel = sim.from_position(dt, lat, lon, t, h=h, p=p, r=r)
Explanation: Run the simulation routine which will interpolate the trajectory and generate inertial readings:
End of explanation
plt.plot(traj_ref.lon, traj_ref.lat)
plt.plot(traj_ref.lon[0], traj_ref.lat[0], 'kx', markersize=12)
plt.xlabel("lon, deg")
plt.ylabel("lat, deg")
Explanation: The final trajectory is drawn below, the initial point is marked with a cross.
End of explanation
from pyins.integrate import coning_sculling, integrate
from pyins.filt import traj_diff
Explanation: Integrating ideal data
Just to check that everything is correct we want to integrate the generated gyro and accel readings.
End of explanation
theta, dv = coning_sculling(gyro, accel)
Explanation: First we apply coning and sculling corrections:
End of explanation
traj_ideal = integrate(dt, *traj_ref.iloc[0], theta, dv)
Explanation: And the run the integration.
End of explanation
err_ideal = traj_diff(traj_ideal, traj_ref)
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()
Explanation: Compute integration error using a convenience function:
End of explanation
plot_errors(dt, err_ideal)
Explanation: We see that attitude and velocity errors are vanishingly small. The position errors are less than 3 meters during 3 hours of operations, which is completely negligible compared to errors of even the most accurate INS.
End of explanation
gyro_bias_sd = np.deg2rad(0.05) / 3600  # 0.05 d/h
accel_bias_sd = 5e-3
gyro_bias_sd
gyro_noise = 1e-6  # rad / s^0.5
accel_noise = 3e-4  # m / s^1.5
Explanation: Integrating "real" data
Now we will run the navigation using inertial sensors with errors.
The error will be a sum of a random bias and additive white noise. We define magnitudes typical for moderately accurate navigation grade sensors.
End of explanation
np.random.seed(1)
gyro_bias = gyro_bias_sd * np.random.uniform(-2, 2, 3)
accel_bias = accel_bias_sd * np.random.uniform(-2, 2, 3)
gyro_bias, accel_bias
from pyins import earth
Explanation: Compute biases as a random constants. To avoid a "bad case" in this example we generated biases uniformly within $[-2 \sigma, 2 \sigma]$.
End of explanation
gyro_e = gyro + gyro_bias * dt + gyro_noise * np.random.randn(*gyro.shape) * dt**0.5
accel_e = accel + accel_bias * dt + accel_noise * np.random.randn(*accel.shape) * dt**0.5
Explanation: Now we apply errors to inertial readings:
End of explanation
theta, dv = coning_sculling(gyro_e, accel_e)
Explanation: Compute coning and sculling corrections:
End of explanation
t_align = 15 * 60
align_samples = int(t_align / dt)
Explanation: An INS operation have to start with the self alignment. We devote 15 minutes of the initial rest for it:
End of explanation
theta_align = theta[:align_samples]
theta_nav = theta[align_samples:]
dv_align = dv[:align_samples]
dv_nav = dv[align_samples:]
from pyins.align import align_wahba
(h0, p0, r0), P_align = align_wahba(dt, theta_align, dv_align, 58)
Explanation: Split the readings into alignment and navigation parts:
End of explanation
h0 - traj_ref.h.loc[align_samples],  p0 - traj_ref.p.loc[align_samples], r0 - traj_ref.r.loc[align_samples]
Explanation: Compare estimated attitude angles with the true angles.
End of explanation
lat0, lon0 = perturb_ll(traj_ref.lat.loc[align_samples], traj_ref.lon.loc[align_samples],
                        10 * np.random.randn(1), 10 * np.random.randn(1))
Explanation: Assume that the initial position is known with the accuracy typical to GPS receivers:
End of explanation
VE0 = 0
VN0 = 0
Explanation: Assume that it is known that the navigation starts at rest ans set initial velocities to 0:
End of explanation
traj_real = integrate(dt, lat0, lon0, VE0, VN0, h0, p0, r0, theta_nav, dv_nav, stamp=align_samples)
traj_error = traj_diff(traj_real, traj_ref)
Explanation: Now we can run the integration:
End of explanation
plot_errors(dt, traj_error)
Explanation: We see that even with very accurate gyros pure INS performance is not that good.
End of explanation
gps_data = pd.DataFrame(index=traj_ref.index[::10])
gps_data['lat'] = traj_ref.lat[::10]
gps_data['lon'] = traj_ref.lon[::10]
Explanation: Aiding from GPS
Now we will use idealize GPS position observations for INS errors estimation and correction.
We assume that GPS is available every second and use known exact values of latitude and longitude:
End of explanation
gps_pos_sd = 10
gps_data['lat'], gps_data['lon'] = perturb_ll(gps_data.lat, gps_data.lon,
                                              gps_pos_sd * np.random.randn(*gps_data.lat.shape),
                                              gps_pos_sd * np.random.randn(*gps_data.lon.shape))
Explanation: We will use an idealized model that GPS observations contain only additive normal errors with a standard deviation of 10 meters (note that in reality errors in outputs from GPS receivers behave much worse).
End of explanation
from pyins.filt import LatLonObs
gps_obs = LatLonObs(gps_data, gps_pos_sd)
Explanation: To use GPS measurements in a navigation Kalman filter we wrap this data into a special object:
End of explanation
from pyins.filt import InertialSensor
gyro_model = InertialSensor(bias=gyro_bias_sd, noise=gyro_noise)
accel_model = InertialSensor(bias=accel_bias_sd, noise=accel_noise)
Explanation: Also define gyro and accelerometer models using parameters defined above:
End of explanation
from pyins.filt import FeedforwardFilter
Explanation: Now we can run a navigation Kalman filter which will blend INS and GPS data. In this example INS errors didn't grow very large, thus we can use a feedforward filter.
End of explanation
ff_filt = FeedforwardFilter(dt, traj_real,
                            pos_sd=10, vel_sd=0.1, azimuth_sd=0.5, level_sd=0.05,
                            gyro_model=gyro_model, accel_model=accel_model)
Explanation: We create a filter by passing sampling period and computed trajectory. To initialize the covariance matrix we pass standard deviations of the initial errors.
Currently the covariance matrix is initialized as diagonal, even though it can be done more rigorously, i.e consider correlations between sensor biases and attitude errors. But my view is that a reliable filter should not depend on such fine details, otherwise it is likely to fail in real conditions. So for the sake of simplicity it is implemented like this for now (can be changed later).
Theoretical attitude accuracy (sd values) from static gyrocompassing in our case is: 0.35 deg for heading (azimuth_sd) and 0.03 deg for pitch and roll (level_sd). Here we set values slightly higher to account for a non-perfect alignment:
End of explanation
ff_res = ff_filt.run(observations=[gps_obs])
Explanation: We run the filter and pass available measurements to it. The return value is the INS trajectory corrected by estimated errors.
End of explanation
filt_error = traj_diff(ff_res.traj, traj_ref)
Explanation: Now we want to investigate errors in the filtered trajectory.
End of explanation
plot_errors(dt, filt_error, step=10)
Explanation: Obviously performance in terms of position and velocity accuracy is very good, but this is sort of expected because GPS provides coordinates directly. 
Attitude angle errors are generally decreasing as well, but the picture is less clear. We want to plot their standard deviation bounds estimated by the filter as well.
End of explanation
plt.figure(figsize=(15, 5))
t_plot = filt_error.index * dt / 3600
plt.subplot(131)
plt.plot(t_plot, filt_error.h, 'b')
plt.plot(t_plot, ff_res.sd.h, 'b--')
plt.plot(t_plot, -ff_res.sd.h, 'b--')
plt.xlabel("time, h")
plt.ylabel("deg")
plt.title("heading error")
plt.subplot(132)
plt.plot(t_plot, filt_error.p, 'b')
plt.plot(t_plot, ff_res.sd.p, 'b--')
plt.plot(t_plot, -ff_res.sd.p, 'b--')
plt.xlabel("time, h")
plt.ylabel("deg")
plt.title("pitch error")
plt.subplot(133)
plt.plot(t_plot,  filt_error.r, 'b')
plt.plot(t_plot, ff_res.sd.r, 'b--')
plt.plot(t_plot, -ff_res.sd.r, 'b--')
plt.xlabel("time, h")
plt.ylabel("deg")
plt.title("roll error")
plt.tight_layout()
Explanation: The return value of FeedforwardFilter contains attributes err, sd, gyro_err, gyro_sd, accel_err, accel_sd for estimated trajectory errors and inertial sensor states and their standard deviations. Below we plot true errors for heading, pitch and roll with their 1-sigma bounds provided by the filter.
Generally we see that the filter's performance is adequate. It can be measured more precisely by Monte-Carlo simulation, but this will not be included in this example.
End of explanation
plt.figure(figsize=(15, 5))
t_plot = filt_error.index[::10] * dt / 3600
gyro_err = ff_res.gyro_err.iloc[::10]
gyro_sd = ff_res.gyro_sd.iloc[::10]
plt.subplot(131)
plt.plot(t_plot, gyro_err.BIAS_1 + gyro_sd.BIAS_1, 'b')
plt.plot(t_plot, gyro_err.BIAS_1 - gyro_sd.BIAS_1, 'b')
plt.hlines(gyro_bias[0], *plt.xlim())
plt.xlabel("time, h")
plt.ylabel("rad/s")
plt.title("Gyro 1 bias")
plt.subplot(132)
plt.plot(t_plot, gyro_err.BIAS_2 + gyro_sd.BIAS_2, 'b')
plt.plot(t_plot, gyro_err.BIAS_2 - gyro_sd.BIAS_2, 'b')
plt.hlines(gyro_bias[1], *plt.xlim())
plt.xlabel("time, h")
plt.ylabel("rad/s")
plt.title("Gyro 2 bias")
plt.subplot(133)
plt.plot(t_plot, gyro_err.BIAS_3 + gyro_sd.BIAS_3, 'b')
plt.plot(t_plot, gyro_err.BIAS_3 - gyro_sd.BIAS_3, 'b')
plt.hlines(gyro_bias[2], *plt.xlim())
plt.xlabel("time, h")
plt.ylabel("rad/s")
plt.title("Gyro 3 bias")
plt.tight_layout()
Explanation: Also it is interesting to assess the filter's sensor bias estimation. Plots below show $\pm \sigma$ bands of gyro bias estimates, the straight line depicts the true value. We see that estimation of gyro biases is quite successful.
End of explanation
plt.figure(figsize=(15, 5))
t_plot = filt_error.index[::10] * dt / 3600
accel_err = ff_res.accel_err.iloc[::10]
accel_sd = ff_res.accel_sd.iloc[::10]
plt.subplot(131)
plt.plot(t_plot, accel_err.BIAS_1 + accel_sd.BIAS_1, 'b')
plt.plot(t_plot, accel_err.BIAS_1 - accel_sd.BIAS_1, 'b')
plt.hlines(accel_bias[0], *plt.xlim())
plt.xlabel("time, h")
plt.ylabel("rad/s")
plt.title("Accel 1 bias")
plt.subplot(132)
plt.plot(t_plot, accel_err.BIAS_2 + accel_sd.BIAS_2, 'b')
plt.plot(t_plot, accel_err.BIAS_2 - accel_sd.BIAS_2, 'b')
plt.hlines(accel_bias[1], *plt.xlim())
plt.xlabel("time, h")
plt.ylabel("rad/s")
plt.title("Accel 2 bias")
plt.subplot(133)
plt.plot(t_plot, accel_err.BIAS_3 + accel_sd.BIAS_3, 'b')
plt.plot(t_plot, accel_err.BIAS_3 - accel_sd.BIAS_3, 'b')
plt.hlines(accel_bias[2], *plt.xlim())
plt.xlabel("time, h")
plt.ylabel("rad/s")
plt.title("Accel 3 bias")
plt.tight_layout()
Explanation: Below the same done for accelerometer biases. Horizontal accelerometer biases are less observable on the given trajectory than gyro biases, and the vertical bias is not observable at all because pitch and roll are held zero.
End of explanation
<END_TASK>