<SYSTEM_TASK:>
Given the following text description, write Python code to implement the functionality described below step by step
<END_TASK>
<USER_TASK:>
Description:
 Comparing rust-fc To Simulation Output
The simulator proccessing code adds realisic noise to the IMU input before sending it to rust-fc.
We'll compare the clean "ideal" simulator numbers to what was actually received by rust-fc
Step1:  Message Receive Time
In JSBSim the IMU messages are requested to be sent at the real IMU rate of 819.2 Hz
Step2:  IMU Noisy Acceleration
Here we see the noise put into the IMU data and the true acceleration.
Step3:  State Tracking
The flight comptuer only knows the Inertial state (acceleration). It keeps track of velocity and altitude by integrating this signal. Here we compare rust-fc internal state to the exact numbers from the simulator.
<ASSISTANT_TASK:>
Python Code:
import psas_packet
from psas_packet.io import BinFile
import csv
import matplotlib.pyplot as plt
from matplotlib import gridspec
%matplotlib inline
FPS2M = 0.3048
LBF2N = 4.44822
LBS2KG = 0.453592
# Extend PSAS Packet to include our state message
psas_packet.messages.MESSAGES["STAT"] = psas_packet.messages.Message({
    'name': "State Vector",
    'fourcc': b'STAT',
    'size': "Fixed",
    'endianness': '!',
    'members': [
        {'key': "time",      'stype': "Q"},
        {'key': "accel",     'stype': "d"},
        {'key': "vel",       'stype': "d"},
        {'key': "alt",       'stype': "d"},
            {'key': "roll_rate",       'stype': "d"},
            {'key': "roll_angle",       'stype': "d"},
    ]
})
# Read data from rust-fc
logfile = BinFile('../logfile-000')
max_acc = 0
rust_time = []
rust_accel_x = []
rust_accel_y = []
rust_accel_z = []
rust_state_time = []
rust_vel = []
rust_alt = []
for fourcc, data in logfile.read():
    if fourcc == 'ADIS':
        if data['Acc_X'] > max_acc:
            max_acc = data['Acc_X']
            rust_t = data['timestamp']/1.0e9
        rust_time.append(data['timestamp']/1.0e9)
        rust_accel_x.append(data['Acc_X'])
        rust_accel_y.append(data['Acc_Y'])
        rust_accel_z.append(data['Acc_Z'])
    if fourcc == 'STAT':
        rust_state_time.append(data['timestamp']/1.0e9)
        rust_vel.append(data['vel'])
        rust_alt.append(data['alt'])
# Read data from JSBSim
max_accel = 0
sim_time = []
measured_accel_x = []
sim_vel_up = []
sim_alt = []
with open('../simulation/data.csv') as datafile:
    reader = csv.reader(datafile, delimiter=',')
    for row in reader:
        # ignore first line
        if row[0][0] == 'T':
            continue
        sim_time.append(float(row[0]))
        force_x = float(row[18]) * LBF2N
        weight = float(row[6]) * LBS2KG
        measured_accel_x.append(force_x/weight)
        if (force_x/weight) > max_accel:
            max_accel = force_x/weight
            sim_t = sim_time[-1]
        sim_vel_up.append(-float(row[10]) * FPS2M)
        sim_alt.append(float(row[2]))
# line up time
sim_offset = rust_t - sim_t
sim_time = [t + sim_offset for t in sim_time]
Explanation: Comparing rust-fc To Simulation Output
The simulator proccessing code adds realisic noise to the IMU input before sending it to rust-fc.
We'll compare the clean "ideal" simulator numbers to what was actually received by rust-fc
End of explanation
# Get the time difference between each ADIS message
diff = [(rust_time[i+1] - t)*1000 for i, t in enumerate(rust_time[:-1])]
fig, ax1 = plt.subplots(figsize=(18,7))
plt.title(r"rust-fc ADIS Message Interval")
plt.ylabel(r"Time Since Last Sample [ms]")
plt.xlabel(r"Sample Number [#]")
plt.plot(range(len(diff)), diff, 'r.', alpha=1.0, ms=0.3, label="rust-fc Sample Interval")
plt.plot((0, len(diff)), (1.2207, 1.2207), 'k-', lw=0.6, alpha=0.7, label="Expected Sample Interval")
ax1.set_yscale("log", nonposy='clip')
plt.ylim([0.1,100])
#plt.xlim()
ax1.legend(loc=1)
plt.show()
fig, ax1 = plt.subplots(figsize=(18,7))
plt.title(r"rust-fc ADIS Message Interval")
plt.ylabel(r"Number of Samples [#]")
plt.xlabel(r"Time Since Last Sample [ms]")
n, bins, patches = plt.hist(diff, 1000, histtype='step', normed=1, alpha=0.8, linewidth=1, fill=True)
plt.plot((1.2207, 1.2207), (0, 1000), 'k-', lw=0.6, alpha=0.7, label="Expected Sample Interval")
plt.ylim([0, 35])
#plt.xlim()
ax1.legend(loc=1)
plt.show()
Explanation: Message Receive Time
In JSBSim the IMU messages are requested to be sent at the real IMU rate of 819.2 Hz:
&lt;output name="localhost" type="SOCKET" protocol="UDP" port="5123" rate="819.2"&gt;
But there they are then processed in python for noise and binary packing. Then it's sent as UDP packets which may get lost. Let's see how they appear in the flight comptuer.
End of explanation
fig, ax1 = plt.subplots(figsize=(18,7))
plt.title(r"rust-fc Recorded IMU Acceleration")
plt.ylabel(r"Acceleration [m/s${}^2$]")
plt.xlabel(r"Run Time [s]")
plt.plot(rust_time, rust_accel_x, alpha=0.8, lw=0.5, label="rust-fc IMU 'Up'")
plt.plot(rust_time, rust_accel_y, alpha=0.8, lw=0.5, label="rust-fc IMU 'Y'")
plt.plot(rust_time, rust_accel_z, alpha=0.6, lw=0.5, label="rust-fc IMU 'Z'")
plt.plot(sim_time, measured_accel_x, 'k-', lw=1.3, alpha=0.6, label="JSBSim True Acceleration")
#plt.ylim()
#plt.xlim()
ax1.legend(loc=1)
plt.show()
Explanation: IMU Noisy Acceleration
Here we see the noise put into the IMU data and the true acceleration.
End of explanation
# Computer difference from FC State and simulation "real" numbers
sim_idx = 0
vel = 0
alt = 0
i_count = 0
sim_matched_vel = []
vel_diff = []
alt_diff = []
for i, t in enumerate(rust_state_time):
    vel += rust_vel[i]
    alt += rust_alt[i]
    i_count += 1
    if sim_time[sim_idx] < t:
        sim_matched_vel.append(vel/float(i_count))
        vel_diff.append(sim_vel_up[sim_idx] - (vel/float(i_count)))
        alt_diff.append(sim_alt[sim_idx] - (alt/float(i_count)))
        vel = 0
        alt = 0
        i_count = 0
        sim_idx += 1
        if sim_idx > len(sim_time)-1:
            break
fig = plt.figure(figsize=(18,9))
plt.subplots_adjust(hspace=0.001)   # no space between vertical charts
gs = gridspec.GridSpec(2, 1, height_ratios=[2, 1]) # stretch main chart to be most of the width
ax1 = plt.subplot(gs[0])
plt.title(r"rust-fc State Tracking: Velocity And Velocity Integration Error")
plt.ylabel(r"Velocity [m/s]")
plt.plot(rust_state_time, rust_vel, alpha=0.8, lw=1.5, label="rust-fc State Vector Velocity")
plt.plot(sim_time, sim_vel_up, 'k-', lw=1.3, alpha=0.6, label="JSBSim True Velocity")
plt.ylim([-60,400])
ticklabels = ax1.get_xticklabels()
plt.setp(ticklabels, visible=False)
ax2 = plt.subplot(gs[1])
plt.xlabel(r"Run Time [s]")
plt.ylabel(r"Integration Drift Error [m/s]")
plt.plot(sim_time, vel_diff)
ax1.legend(loc=1)
plt.show()
fig = plt.figure(figsize=(18,9))
plt.subplots_adjust(hspace=0.001)   # no space between vertical charts
gs = gridspec.GridSpec(2, 1, height_ratios=[2, 1]) # stretch main chart to be most of the width
ax1 = plt.subplot(gs[0])
plt.title(r"rust-fc State Tracking: Altitude And ALtitude Integration Error")
plt.ylabel(r"Altitude MSL [m]")
plt.plot(rust_state_time, rust_alt, alpha=0.8, lw=1.5, label="rust-fc State Vector Altitude")
plt.plot(sim_time, sim_alt, 'k-', lw=1.3, alpha=0.6, label="JSBSim True Velocity")
plt.ylim([1390, 7500])
ticklabels = ax1.get_xticklabels()
plt.setp(ticklabels, visible=False)
ax2 = plt.subplot(gs[1])
plt.xlabel(r"Run Time [s]")
plt.ylabel(r"Integration Drift Error [m]")
plt.plot(sim_time, alt_diff)
#plt.xlim()
ax1.legend(loc=1)
plt.show()
Explanation: State Tracking
The flight comptuer only knows the Inertial state (acceleration). It keeps track of velocity and altitude by integrating this signal. Here we compare rust-fc internal state to the exact numbers from the simulator.
End of explanation
<END_TASK>