autotune: compensate for airspeed to produce a model at trim airspeed

Author: bresch

autotune/autotune.py

@@ -57,7 +57,8 @@ class Window(QDialog):
     def __init__(self, parent=None):
         super(Window, self).__init__(parent)
 
-        self._plot_ref = None
+        self.model_ref = None
+        self.input_ref = None
         self.closed_loop_ref = None
         self.closed_loop_ax = None
         self.bode_plot_ref = None
@@ -124,6 +125,16 @@ def __init__(self, parent=None):
         layout_tf = self.createTfLayout()
         left_menu.addLayout(layout_tf)
 
+
+        trim_group = QFormLayout()
+        self.line_edit_trim = QDoubleSpinBox()
+        self.trim_airspeed = 20.0
+        self.line_edit_trim.setValue(self.trim_airspeed)
+        self.line_edit_trim.setRange(0.0, 100.0)
+        self.line_edit_trim.textChanged.connect(self.onTrimChanged)
+        trim_group.addRow(QLabel("Trim airspeed"), self.line_edit_trim)
+        left_menu.addLayout(trim_group)
+
         offset_group = QFormLayout()
         self.line_edit_offset = QDoubleSpinBox()
         self.line_edit_offset.setValue(0.0)
@@ -151,7 +162,8 @@ def __init__(self, parent=None):
         self.setLayout(layout_v)
 
     def reset(self):
-        self._plot_ref = None
+        self.model_ref = None
+        self.input_ref = None
         self.closed_loop_ref = None
         self.bode_plot_ref = None
         self.state_plot_refs= []
@@ -206,6 +218,16 @@ def updateCoeffTable(self):
     def onModelChanged(self):
         self.btn_update_model.setEnabled(True)
 
+    def onTrimChanged(self):
+        try:
+            self.trim_airspeed = float(self.line_edit_trim.text())
+        except ValueError:
+            self.trim_airspeed = 0
+            self.line_edit_trim.setValue(self.trim_airspeed)
+
+        self.btn_run_sys_id.setEnabled(True)
+        self.plotInputOutput()
+
     def onOffsetChanged(self):
         self.plotInputOutput()
 
@@ -543,17 +565,13 @@ def updateClosedLoop(self):
         kd = self.kd
         kff = self.kff
 
-        airspeed = 40.0
-        airspeed_trim = 38.0
-        airspeed_scale = airspeed_trim / airspeed
         delays = ctrl.TransferFunction([1], np.append([1], np.zeros(self.sys_id_delays)), dt, inputs='r', outputs='rd')
         plant = ctrl.TransferFunction(num, den, dt, inputs='u', outputs='plant_out')
         sampler = ctrl.TransferFunction([1], [1, 0], dt, inputs='plant_out', outputs='y')
         sum_feedback = ctrl.summing_junction(inputs=['rd', '-y'], output='e')
 
         # Default is standard PID
         feedforward = ctrl.TransferFunction([kff], [1], dt, inputs='rd', outputs='ff_out')
-        ff_scale = ctrl.TransferFunction([airspeed_scale], [1], inputs='ff_out', outputs='ff_out_scaled')
         i_control = ctrl.TransferFunction([ki * dt, ki * dt], [2, -2], dt, inputs='e', outputs='i_out') # Integrator discretized using bilinear transform: s = 2(z-1)/(dt(z+1))
 
         # Derivative with 1st order LPF (discretized using Euler method: s = (z-1)/dt)
@@ -565,8 +583,7 @@ def updateClosedLoop(self):
 
         id_control = ctrl.summing_junction(inputs=['e', 'i_out', 'd_out'], output='id_out')
         p_control = ctrl.TransferFunction([kc], [1], dt, inputs='id_out', outputs='pid_out')
-        pid_scale = ctrl.TransferFunction([airspeed_scale**2], [1], inputs='pid_out', outputs='pid_out_scaled')
-        sum_control = ctrl.summing_junction(inputs=['pid_out_scaled', 'ff_out_scaled'], output='u')
+        sum_control = ctrl.summing_junction(inputs=['pid_out', 'ff_out'], output='u')
 
         remove_zero = False
         no_derivative_kick = True
@@ -579,7 +596,7 @@ def updateClosedLoop(self):
             # Derivative on feedback only to remove the "derivative kick"
             d_control = ctrl.TransferFunction(-derivative_num, derivative_den, dt, inputs='y', outputs='d_out')
 
-        closed_loop = ctrl.interconnect([delays, sampler, sum_feedback, feedforward, sum_control, p_control, i_control, d_control, id_control, pid_scale, ff_scale, plant], inputs='r', outputs='y')
+        closed_loop = ctrl.interconnect([delays, sampler, sum_feedback, feedforward, sum_control, p_control, i_control, d_control, id_control, plant], inputs='r', outputs='y')
 
         t_out,y_out = ctrl.step_response(closed_loop, T=np.arange(0,1,dt))
         self.plotClosedLoop(t_out, y_out)
@@ -624,28 +641,35 @@ def plotBode(self, w, mag, w_cl, mag_cl):
         self.canvas.draw()
 
     def plotInputOutput(self, redraw=False):
-        if self._plot_ref is None or redraw:
+        if len(self.true_airspeed) == len(self.input):
+            scale = np.array(self.true_airspeed) / self.trim_airspeed
+            self.u = self.input * scale**2
+
+        if self.model_ref is None or redraw:
             # First time we have no plot reference, so do a normal plot.
             # .plot returns a list of line <reference>s, as we're
             # only getting one we can take the first element.
             self.figure.clear()
             ax = self.figure.add_subplot(3,3,(1,3))
-            ax.plot(self.t, self.u, self.t, self.y)
+            input_ref = ax.plot(self.t, self.u)
+            self.input_ref = input_ref[0]
+            ax.plot(self.t, self.y)
             plot_refs = ax.plot(0, 0)
-            self._plot_ref = plot_refs[0]
+            self.model_ref = plot_refs[0]
             ax.set_title("Logged data")
             ax.set_xlabel("Time (s)")
             ax.set_ylabel("Amplitude")
             ax.legend(["Input", "Output", "Model"])
         else:
             # We have a reference, we can use it to update the data for that line.
-            self._plot_ref.set_xdata(self.t_est)
+            self.model_ref.set_xdata(self.t_est)
             try:
                 offset = float(self.line_edit_offset.text())
             except ValueError:
                 offset = 0
 
-            self._plot_ref.set_ydata(self.y_est + offset)
+            self.model_ref.set_ydata(self.y_est + offset)
+            self.input_ref.set_ydata(self.u)
 
         self.canvas.draw()
 
@@ -660,8 +684,10 @@ def loadLog(self):
             if select.exec_():
                     self.reset()
                     self.t = select.t - select.t[0]
-                    self.u = select.u
+                    self.input = select.u
+                    self.u = self.input
                     self.y = select.y
+                    self.true_airspeed = select.v
                     self.refreshInputOutputData()
                     self.runIdentification()
                     self.computeController()
@@ -678,6 +704,8 @@ def resampleData(self, dt):
             self.t = np.arange(0, self.t[-1]+self.dt, self.dt)
             self.u = resample(self.u, len(self.t))
             self.y = resample(self.y, len(self.t))
+            self.true_airspeed = resample(self.true_airspeed, len(self.t))
+            self.input = resample(self.input, len(self.t))
 
 class DoubleSlider(QSlider):
 

autotune/data_extractor.py

@@ -50,15 +50,18 @@ def getInputOutputData(logfile, axis, t_start=0.0, t_stop=0.0, instance=0):
     u_data = get_data(log, 'vehicle_torque_setpoint', 'xyz[{}]'.format(axis))
     t_u_data = us2s(get_data(log, 'vehicle_torque_setpoint', 'timestamp'))
 
+    v_data = get_data(log, 'airspeed_validated', 'true_airspeed_m_s')
+    t_v_data = us2s(get_data(log, 'airspeed_validated', 'timestamp'))
+
     if not np.any(u_data):
         # Check for legacy topics
         actuator_controls_n = 'actuator_controls_{}'.format(instance)
         u_data = get_data(log, actuator_controls_n, 'control[{}]'.format(axis))
         t_u_data = us2s(get_data(log, actuator_controls_n, 'timestamp'))
 
-    (t_aligned, u_aligned, y_aligned) = extract_identification_data(log, t_u_data, u_data, t_y_data, y_data, axis, t_start, t_stop)
+    (t_aligned, u_aligned, y_aligned, v_aligned) = extract_identification_data(log, t_u_data, u_data, t_y_data, y_data, axis, t_v_data, v_data, t_start, t_stop)
 
-    return (t_aligned, u_aligned, y_aligned)
+    return (t_aligned, u_aligned, y_aligned, v_aligned)
 
 def get_data(log, topic_name, variable_name, instance=0):
     variable_data = np.array([])
@@ -82,16 +85,19 @@ def get_delta_mean(data_list):
     dx = dx/(length-1)
     return dx
 
-def extract_identification_data(log, t_u_data, u_data, t_y_data, y_data, axis, t_start, t_stop):
+def extract_identification_data(log, t_u_data, u_data, t_y_data, y_data, axis, t_v_data, v_data, t_start, t_stop):
     status_data = get_data(log, 'autotune_attitude_control_status', 'state')
     t_status = us2s(get_data(log, 'autotune_attitude_control_status', 'timestamp'))
 
     len_y = len(t_y_data)
     len_s = len(t_status)
+    len_v = len(t_v_data)
     i_y = 0
     i_s = 0
+    i_v = 0
     u_aligned = []
     y_aligned = []
+    v_aligned = []
     t_aligned = []
     axis_to_state = [2, 4, 6] # roll, pitch, yaw states
 
@@ -106,6 +112,9 @@ def extract_identification_data(log, t_u_data, u_data, t_y_data, y_data, axis, t
             while t_y_data[i_y] <= t_u and i_y < len_y-1:
                 i_y += 1
 
+            while i_v < len_v-1 and t_v_data[i_v] <= t_u:
+                i_v += 1
+
             if len_s > 0:
                 while t_status[i_s] <= t_u and i_s < len_s-1:
                     i_s += 1
@@ -117,12 +126,18 @@ def extract_identification_data(log, t_u_data, u_data, t_y_data, y_data, axis, t
                     y_aligned.append(y_data[i_y-1])
                     t_aligned.append(t_u)
 
+                    if i_v > 0:
+                        v_aligned.append(v_data[i_v-1])
+
             elif t_u >= t_start and t_u <= t_stop:
                 u_aligned.append(u_data[i_u])
                 y_aligned.append(y_data[i_y-1])
                 t_aligned.append(t_u)
 
-    return (t_aligned, u_aligned, y_aligned)
+                if i_v > 0:
+                    v_aligned.append(v_data[i_v-1])
+
+    return (t_aligned, u_aligned, y_aligned, v_aligned)
 
 def printCppArrays(t_aligned, u_aligned, y_aligned):
     # Print data in c++ arrays
@@ -166,5 +181,5 @@ def printCppArrays(t_aligned, u_aligned, y_aligned):
     logfile = os.path.abspath(args.logfile) # Convert to absolute path
     axis = {'x':0, 'y':1, 'z':2}[args.axis]
 
-    (t_aligned, u_aligned, y_aligned) = getInputOutputData(logfile, axis, instance=0)
+    (t_aligned, u_aligned, y_aligned, v_aligned) = getInputOutputData(logfile, axis, instance=0)
     printCppArrays(t_aligned, u_aligned, y_aligned)

autotune/data_selection_window.py

@@ -46,7 +46,7 @@ def __init__(self, filename):
 
     def loadLog(self):
         if self.t_stop > self.t_start:
-            (self.t, self.u, self.y) = getInputOutputData(self.file_name, self.axis, self.t_start, self.t_stop)
+            (self.t, self.u, self.y, self.v) = getInputOutputData(self.file_name, self.axis, self.t_start, self.t_stop)
             self.accept()
         else:
             self.printRangeError()
@@ -73,7 +73,7 @@ def loadZData(self):
     def refreshInputOutputData(self, axis=0):
         if self.file_name:
             self.axis = axis
-            (self.t, self.u, self.y) = getInputOutputData(self.file_name, axis)
+            (self.t, self.u, self.y, self.v) = getInputOutputData(self.file_name, axis)
             self.plotInputOutput(redraw=True)
 
     def plotInputOutput(self, redraw=False):