[tool] update effectiveness estimation script (#3537)

- proper initial condition for linear filters
- add rate limit model
- possibility to use ranges to find best value for actuator dynamic
- allow to change any of the variables from command line
- update plot figures to group them with signal, fit result and fft
- add config files for full INDI with G2 matrix and Heewing Ranger T1 configuration

sw/airborne/modules/loggers/logger_control_effectiveness.c

@@ -53,13 +53,8 @@
 // extra includes
 
 #if LOGGER_CONTROL_EFFECTIVENESS_COMMANDS
-#ifdef ROTORCRAFT_FIRMWARE
-#include "firmwares/rotorcraft/stabilization.h"
-#endif
-#ifdef FIXEDWING_FIRMWARE
 #include "modules/core/commands.h"
 #endif
-#endif
 
 #if LOGGER_CONTROL_EFFECTIVENESS_ACTUATORS
 #include "modules/actuators/actuators.h"
@@ -117,12 +112,7 @@ void logger_control_effectiveness_periodic(void)
   // log commands
 #if LOGGER_CONTROL_EFFECTIVENESS_COMMANDS
   for (unsigned int i = 0; i < COMMANDS_NB; i++) {
-#ifdef ROTORCRAFT_FIRMWARE
-    sdLogWriteLog(pprzLogFile, ",%ld", stabilization.cmd[i]);
-#endif
-#ifdef FIXEDWING_FIRMWARE
     sdLogWriteLog(pprzLogFile, ",%d", commands[i]);
-#endif
   }
 #endif
 

sw/tools/control_effectiveness/config/quad_full.json

@@ -0,0 +1,173 @@
+{
+  "variables" : {
+    "filt_cutoff" : 4.0,
+    "act_freq" : 15.0
+  },
+  "ranges" : {
+    "act_freq" : [10.0, 40.0, 0.2]
+  },
+  "data" : [
+    {
+      "name" : "time",
+      "type" : "timestamp",
+      "column" : 0,
+      "index" : -1,
+      "format" : "float",
+      "scale" : 1.0,
+      "unit" : "s"
+    },
+    {
+      "name" : "rate.x",
+      "type" : "input",
+      "column" : 1,
+      "index" : 0,
+      "format" : "bfp",
+      "resolution" : 12,
+      "filters" : [["butter", [2, "filt_cutoff"]], ["diff_signal", [2]]]
+    },
+    {
+      "name" : "rate.y",
+      "type" : "input",
+      "column" : 2,
+      "index" : 1,
+      "format" : "bfp",
+      "resolution" : 12,
+      "filters" : [["butter", [2, "filt_cutoff"]], ["diff_signal", [2]]]
+    },
+    {
+      "name" : "rate.z",
+      "type" : "input",
+      "column" : 3,
+      "index" : 2,
+      "format" : "bfp",
+      "resolution" : 12,
+      "filters" : [["butter", [2, "filt_cutoff"]], ["diff_signal", [2]]]
+    },
+    {
+      "name" : "accel.x",
+      "type" : "input",
+      "column" : 4,
+      "index" : -1,
+      "format" : "bfp",
+      "resolution" : 10,
+      "filters" : []
+    },
+    {
+      "name" : "accel.y",
+      "type" : "input",
+      "column" : 5,
+      "index" : -1,
+      "format" : "bfp",
+      "resolution" : 10,
+      "filters" : []
+    },
+    {
+      "name" : "accel.z",
+      "type" : "input",
+      "column" : 6,
+      "index" : 3,
+      "format" : "bfp",
+      "resolution" : 10,
+      "filters" : [["butter", [2, "filt_cutoff"]], ["diff_signal", [1]]]
+    },
+    {
+      "name" : "act_1",
+      "type" : "command",
+      "column" : 11,
+      "index" : 0,
+      "format" : "pprz",
+      "filters" : [["1st_order", ["act_freq"]], ["butter", [2, "filt_cutoff"]], ["diff_signal", [1]]]
+    },
+    {
+      "name" : "act_2",
+      "type" : "command",
+      "column" : 12,
+      "index" : 1,
+      "format" : "pprz",
+      "filters" : [["1st_order", ["act_freq"]], ["butter", [2, "filt_cutoff"]], ["diff_signal", [1]]]
+    },
+    {
+      "name" : "act_3",
+      "type" : "command",
+      "column" : 13,
+      "index" : 2,
+      "format" : "pprz",
+      "filters" : [["1st_order", ["act_freq"]], ["butter", [2, "filt_cutoff"]], ["diff_signal", [1]]]
+    },
+    {
+      "name" : "act_4",
+      "type" : "command",
+      "column" : 14,
+      "index" : 3,
+      "format" : "pprz",
+      "filters" : [["1st_order", ["act_freq"]], ["butter", [2, "filt_cutoff"]], ["diff_signal", [1]]]
+    },
+    {
+      "name" : "act_g2_1",
+      "type" : "command",
+      "column" : 11,
+      "index" : 4,
+      "format" : "pprz",
+      "filters" : [["1st_order", ["act_freq"]], ["butter", [2, "filt_cutoff"]], ["diff_signal", [2]], ["div", ["freq"]]]
+    },
+    {
+      "name" : "act_g2_2",
+      "type" : "command",
+      "column" : 12,
+      "index" : 5,
+      "format" : "pprz",
+      "filters" : [["1st_order", ["act_freq"]], ["butter", [2, "filt_cutoff"]], ["diff_signal", [2]], ["div", ["freq"]]]
+    },
+    {
+      "name" : "act_g2_3",
+      "type" : "command",
+      "column" : 13,
+      "index" : 6,
+      "format" : "pprz",
+      "filters" : [["1st_order", ["act_freq"]], ["butter", [2, "filt_cutoff"]], ["diff_signal", [2]], ["div", ["freq"]]]
+    },
+    {
+      "name" : "act_g2_4",
+      "type" : "command",
+      "column" : 14,
+      "index" : 7,
+      "format" : "pprz",
+      "filters" : [["1st_order", ["act_freq"]], ["butter", [2, "filt_cutoff"]], ["diff_signal", [2]], ["div", ["freq"]]]
+    }
+  ],
+  "mixing" : [
+    [ 1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0 ],
+    [ 1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0 ],
+    [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 ],
+    [ 1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0 ]
+  ],
+  "display" : [
+    {
+      "name" : "G1",
+      "matrix": [
+        [[0,0],[0,1],[0,2],[0,3]],
+        [[1,0],[1,1],[1,2],[1,3]],
+        [[2,0],[2,1],[2,2],[2,3]],
+        [[3,0],[3,1],[3,2],[3,3]]
+      ],
+      "scaling" : 1000
+    },
+    {
+      "name" : "G2",
+      "coef" : [[2,4],[2,5],[2,6],[2,7]],
+      "scaling" : 1000
+    },
+    {
+      "name" : "FILT_CUTOFF",
+      "coef" : "filt_cutoff"
+    },
+    {
+      "name" : "FILT_CUTOFF_R",
+      "coef" : "filt_cutoff"
+    },
+    {
+      "name" : "ACT_FREQ",
+      "coef" : ["act_freq","act_freq","act_freq","act_freq"]
+    }
+  ]
+}

sw/tools/control_effectiveness/config/ranger_t1.json

@@ -0,0 +1,144 @@
+{
+  "variables" : {
+    "filt_cutoff" : 5.0,
+    "act_freq" : 17.5,
+    "elevon_freq" : 40.0,
+    "servo_freq" : 22.0,
+    "servo_rate" : 70
+  },
+  "ranges" : {
+    "act_freq" : [10.0, 30.0, 0.5],
+    "servo_freq" : [10.0, 40.0, 0.5]
+  },
+  "data" : [
+    {
+      "name" : "time",
+      "type" : "timestamp",
+      "column" : 0,
+      "index" : -1,
+      "format" : "float",
+      "scale" : 1.0,
+      "unit" : "s"
+    },
+    {
+      "name" : "rate.x",
+      "type" : "input",
+      "column" : 1,
+      "index" : 0,
+      "format" : "bfp",
+      "resolution" : 12,
+      "filters" : [["butter", [2, "filt_cutoff"]], ["diff_signal", [2]]]
+    },
+    {
+      "name" : "rate.y",
+      "type" : "input",
+      "column" : 2,
+      "index" : 1,
+      "format" : "bfp",
+      "resolution" : 12,
+      "filters" : [["butter", [2, "filt_cutoff"]], ["diff_signal", [2]]]
+    },
+    {
+      "name" : "rate.z",
+      "type" : "input",
+      "column" : 3,
+      "index" : 2,
+      "format" : "bfp",
+      "resolution" : 12,
+      "filters" : [["butter", [2, "filt_cutoff"]], ["diff_signal", [2]]]
+    },
+    {
+      "name" : "accel.x",
+      "type" : "input",
+      "column" : 4,
+      "index" : -1,
+      "format" : "bfp",
+      "resolution" : 10,
+      "filters" : []
+    },
+    {
+      "name" : "accel.y",
+      "type" : "input",
+      "column" : 5,
+      "index" : -1,
+      "format" : "bfp",
+      "resolution" : 10,
+      "filters" : []
+    },
+    {
+      "name" : "accel.z",
+      "type" : "input",
+      "column" : 6,
+      "index" : 3,
+      "format" : "bfp",
+      "resolution" : 10,
+      "filters" : [["butter", [2, "filt_cutoff"]], ["diff_signal", [1]]]
+    },
+    {
+      "name" : "motor_right",
+      "type" : "command",
+      "column" : 7,
+      "index" : 0,
+      "format" : "pprz",
+      "filters" : [["1st_order", ["act_freq"]], ["butter", [2, "filt_cutoff"]], ["diff_signal", [1]]]
+    },
+    {
+      "name" : "motor_left",
+      "type" : "command",
+      "column" : 8,
+      "index" : 1,
+      "format" : "pprz",
+      "filters" : [["1st_order", ["act_freq"]], ["butter", [2, "filt_cutoff"]], ["diff_signal", [1]]]
+    },
+    {
+      "name" : "motor_tail",
+      "type" : "command",
+      "column" : 9,
+      "index" : 2,
+      "format" : "pprz",
+      "filters" : [["1st_order", ["act_freq"]], ["butter", [2, "filt_cutoff"]], ["diff_signal", [1]]]
+    },
+    {
+      "name" : "delta_tilt",
+      "type" : "command",
+      "column" : 15,
+      "index" : 3,
+      "format" : "pprz",
+      "filters" : [["1st_order", ["servo_freq"]], ["rate_limit", ["servo_rate"]], ["butter", [2, "filt_cutoff"]], ["diff_signal", [1]]]
+    }
+  ],
+  "mixing" : [
+    [ 1.0, 1.0, 0.0, 0.0 ],
+    [ 1.0, 1.0, 1.0, 0.0 ],
+    [ 0.0, 0.0, 0.0, 1.0 ],
+    [ 1.0, 1.0, 1.0, 0.0 ]
+  ],
+  "display" : [
+    {
+      "name" : "G1",
+      "matrix": [
+        [[0,0],[0,1],[0,2],[0,3]],
+        [[1,0],[1,1],[1,2],[1,3]],
+        [[2,0],[2,1],[2,2],[2,3]],
+        [[3,0],[3,1],[3,2],[3,3]]
+      ],
+      "scaling" : 1000
+    },
+    {
+      "name" : "FILT_CUTOFF",
+      "coef" : "filt_cutoff"
+    },
+    {
+      "name" : "ACT_FREQ",
+      "coef" : ["act_freq","act_freq","act_freq","servo_freq"]
+    },
+    {
+      "name" : "ACT_RATE_LIMIT",
+      "coef" : [9600,9600,9600,"servo_rate"]
+    },
+    {
+      "name" : "ACT_IS_SERVO",
+      "coef" : [0,0,0,1]
+    }
+  ]
+}

sw/tools/control_effectiveness/control_effectiveness_estimation.py

@@ -28,7 +28,7 @@ import matplotlib.pyplot as plt
 
 import control_effectiveness_utils as ut
 
-def process_data(conf, f_name, start, end, freq=None, act_freq=None, verbose=False, plot=False):
+def process_data(conf, f_name, start, end, freq=None, variables=None, verbose=False, use_ranges=False, plot=False):
 
     # Read data from log file
     data = genfromtxt(f_name, delimiter=',', skip_header=1)
@@ -38,8 +38,21 @@ def process_data(conf, f_name, start, end, freq=None, act_freq=None, verbose=Fal
     var = {}
     if 'variables' in conf:
         var = conf['variables']
-    if act_freq is not None:
-        var['act_freq'] = act_freq # this may overwrite default value
+    if variables is not None:
+        # overwrite default value if needed
+        for var_name, value in variables:
+            if var_name not in var:
+                print(f"Variable name '{var_name}' not in list '{var.keys()}'")
+                break
+            try:
+                var[var_name] = float(value)
+            except:
+                print(f"Variable value '{value}' not a float or int")
+
+    # extract ranges
+    ranges = None
+    if 'ranges' in conf and use_ranges:
+        ranges = conf['ranges']
 
     # Get number of inputs and outputs
     mixing = np.array(conf['mixing'])
@@ -74,85 +87,59 @@ def process_data(conf, f_name, start, end, freq=None, act_freq=None, verbose=Fal
         time = np.arange(end-start) / freq # default time vector if not in data
     var['freq'] = freq
 
-    # Search and filter inputs and outputs
-    inputs = np.zeros((end-start, nb_in))
-    commands = np.zeros((end-start, nb_out))
     output = np.zeros((nb_in, nb_out))
-    for el in conf['data']:
-        t = el['type']
-        idx = el['index']
-        col = el['column']
-        if t == 'input' and idx >= 0:
-            if idx >= nb_in:
-                print("Invalid input index for {}".format(el['name']))
-                exit(1)
-            inputs[:, idx] = ut.apply_format(el, data[start:end, col])
-            for (filt_name, params) in el['filters']:
-                inputs[:,idx] = ut.apply_filter(filt_name, params, inputs[:, idx].copy(), var)
-        if t == 'command' and idx >= 0:
-            if idx >= nb_out:
-                print("Invalid command index for {}".format(el['name']))
-                exit(1)
-            commands[:, idx] = ut.apply_format(el, data[start:end, col])
-            for (filt_name, params) in el['filters']:
-                commands[:,idx] = ut.apply_filter(filt_name, params, commands[:, idx].copy(), var)
 
-    for i in range(nb_in):
-        name = ut.get_name_by_index(conf, 'input', i)
-        cmd = np.multiply(commands, mixing[[i],:])
-        fit = ut.fit_axis(cmd, inputs[:,[i]], name, 0, N, verbose)
-        output[[i],:] = fit.T
-        cmd_fit = np.dot(cmd, fit)
-        ut.plot_results(cmd_fit, inputs[:,[i]], time, 0, N, name)
+    if ranges is None:
+        # Search and filter inputs and outputs
+        inputs, raw_inputs, commands, raw_commands = ut.extract_filtered_data(conf, var, data, nb_in, nb_out, start, end)
 
-    try:
-        # display if needed
-        disp = conf['display']
-    except:
-        disp = None
-        print("No display section")
+        for i in range(nb_in):
+            name = ut.get_name_by_index(conf, 'input', i)
+            cmd = np.multiply(commands, mixing[[i],:])
+            axis_fit = ut.fit_axis(cmd, inputs[:,[i]], name, verbose)
+            output[[i],:] = axis_fit.T
+            cmd_fit = np.dot(cmd, axis_fit)
+            lin_fit, res = ut.fit_lin(cmd_fit[:,0], inputs[:,[i]][:,0], name, True)
+            ut.plot_results(cmd_fit, inputs[:,[i]], raw_inputs[:,[i]], lin_fit, time, freq, name)
 
-    if disp is not None:
-        print("\nAdd the following lines to your airframe file:\n")
-        def value_or_default(key, dic, default):
-            if key in dic:
-                return dic[key]
-            else:
-                return default
-        for d in disp:
-            name = d['name']
-            coef = value_or_default('coef', d, None)
-            matrix = value_or_default('matrix', d, None)
-            scaling = value_or_default('scaling', d, 1.)
-            if matrix is not None:
-                print(f'<define name="{name}" type="matrix">')
-                for i in range(len(matrix)):
-                    l = []
-                    for e in matrix[i]:
-                        if isinstance(e, (int, float, str)):
-                            l.append("{:.2f}".format(scaling*ut.get_param(e, var)))
-                        else:
-                            l.append("{:.2f}".format(scaling*output[e[0], e[1]]))
-                    print(f'  <field value="{l}" type="float[]"/>')
-                print('</define>')
-            elif coef is not None:
-                if isinstance(coef, (int, float, str)):
-                    print('<define name="{}" value="{}"/>'.format(name, scaling*ut.get_param(coef, var)))
-                elif len(coef) == 2 and isinstance(coef[0], int) and isinstance(coef[1], int):
-                    print('<define name="{}" value="{:.2f}"/>'.format(name, scaling*output[coef[0], coef[1]]))
-                else:
-                    l = []
-                    for e in coef:
-                        if isinstance(e, (int, float, str)):
-                            l.append("{:.2f}".format(scaling*ut.get_param(e, var)))
-                        else:
-                            l.append("{:.2f}".format(scaling*output[e[0], e[1]]))
-                    print('<define name="{}" value="{}" type="float[]"/>'.format(name, ', '.join(l)))
+    else:
+        for e in ranges:
+            r = ranges[e]
+            lin_var = np.arange(r[0], r[1]+r[2], r[2])
+            lin_res = np.zeros(lin_var.shape)
+            best = np.inf
+            best_result = None
+            tmp_output = np.zeros((nb_in, nb_out))
+            for j, v in enumerate(lin_var):
+                var[e] = v
+                inputs, raw_inputs, commands, raw_commands = ut.extract_filtered_data(
+                        conf, var, data, nb_in, nb_out, start, end)
+                res_total = 0.
+                for i in range(nb_in):
+                    name = ut.get_name_by_index(conf, 'input', i)
+                    cmd = np.multiply(commands, mixing[[i],:])
+                    axis_fit = ut.fit_axis(cmd, inputs[:,[i]], name, verbose)
+                    tmp_output[[i],:] = axis_fit.T
+                    cmd_fit = np.dot(cmd, axis_fit)
+                    lin_fit, residual = ut.fit_lin(cmd_fit[:,0], inputs[:,[i]][:,0], name, verbose)
+                    res_total += float(residual[0])
+
+                if res_total < best:
+                    best = res_total
+                    best_result = v
+                    output = tmp_output.copy()
+                lin_res[j] = res_total
+
+            # show results for this range
+            print(f'Best result for {e} with value {best_result:.2f} (res = {best:.5E})')
+            ut.plot_residuals(lin_var, lin_res, e)
+            var[e] = best_result # set best value in variables
+
+    ut.print_results(conf, var, output)
 
     if plot:
         plt.show()
 
-
 def main():
     from argparse import ArgumentParser
     import json
@@ -160,10 +147,11 @@ def main():
     parser = ArgumentParser(description="Control effectiveness estimation tool")
     parser.add_argument("config", help="JSON configuration file")
     parser.add_argument("data", help="Log file for parameter estimation")
-    parser.add_argument("-sf", "--sample_freq", dest="freq",
+    parser.add_argument("-f", "--sample_freq", dest="freq",
                       help="Sampling frequency, trying auto freq if not set")
-    parser.add_argument("-af", "--act_freq", dest="dyn",
-                      help="First order actuator frequency, 'None' for config file default")
+    parser.add_argument("-var", "--variable", dest="vars", action='append', nargs=2,
+                      metavar=('var_name','value'),
+                      help="Set variables by name, 'None' for config file default")
     parser.add_argument("-s", "--start",
                       help="Start time",
                       action="store", dest="start", default="0")
@@ -173,6 +161,8 @@ def main():
     parser.add_argument("-p", "--plot",
                       help="Show resulting plots",
                       action="store_true", dest="plot")
+    parser.add_argument("-r", "--use_ranges",
+                      action="store_true", dest="use_ranges")
     parser.add_argument("-v", "--verbose",
                       action="store_true", dest="verbose")
     args = parser.parse_args()
@@ -186,13 +176,10 @@ def main():
     freq = args.freq
     if freq is not None:
         freq = float(freq)
-    dyn = args.dyn
-    if dyn is not None:
-        dyn = float(dyn)
 
     with open(args.config, 'r') as f:
         conf = json.load(f)
-        process_data(conf, args.data, start, end, freq, dyn, args.verbose, args.plot)
+        process_data(conf, args.data, start, end, freq, args.vars, args.verbose, args.use_ranges, args.plot)
 
 
 if __name__ == "__main__":

sw/tools/control_effectiveness/control_effectiveness_utils.py

@@ -27,6 +27,7 @@ import sys
 import numpy as np
 import scipy as sp
 from scipy import signal
+from scipy.fftpack import fft
 import matplotlib.pyplot as plt
 from matplotlib.pyplot import show
 
@@ -39,13 +40,19 @@ def first_order_model(signal, freq, cutoff_freq):
     Apply a first order filter with cutoff freq
     '''
     tau = 1. - np.exp(-cutoff_freq / freq)
-    return sp.signal.lfilter([tau], [1, tau-1], signal, axis=0)
+    zi = sp.signal.lfilter_zi([tau], [1, tau-1])
+    out, _ = sp.signal.lfilter([tau], [1, tau-1], signal, zi=signal[0]*zi, axis=0)
+    return out
 
 def rate_limit_model(signal, max_rate):
     '''
     Apply rate limiter of signal
     '''
-    return signal # TODO
+    out = np.full(signal.size, signal[0])
+    for i in range(len(signal)-1):
+        delta = signal[i] - out[i]
+        out[i+1] = out[i] + np.sign(delta) * min(max_rate, np.abs(delta))
+    return out
 
 #
 # Utility functions
@@ -56,9 +63,8 @@ def diff_signal(signal, freq, order=1, filt=None):
     compute the nth-order derivative of a signal of fixed freq
     and by applying a filter if necessary
     '''
-    diff = np.diff(signal, order)
-    res = np.hstack((np.zeros((1,order)), diff.reshape(1,len(diff)))) * pow(freq, order)
-    return res
+    diff = np.diff(signal, order, prepend=np.full((order,),signal[0]))
+    return diff * pow(freq, order)
 
 def get_param(param, var):
     '''
@@ -92,7 +98,9 @@ def apply_filter(filt_name, params, signal, var):
     elif filt_name == 'butter':
         # params = [order, Wn]
         b, a = sp.signal.butter(get_param(params[0], var), get_param(params[1], var)/(var['freq']/2))
-        return sp.signal.lfilter(b, a, signal, axis=0)
+        zi = sp.signal.lfilter_zi(b, a)
+        out, _ = sp.signal.lfilter(b, a, signal, zi=zi*signal[0], axis=0)
+        return out
 
     elif filt_name == 'mult':
         # params = [factor]
@@ -151,39 +159,147 @@ def get_index_from_time(time, start, end):
         end_idx = N
     return (start_idx, end_idx)
 
+def extract_filtered_data(conf, var, data, nb_in, nb_out, start, end):
+    inputs = np.zeros((end-start, nb_in))
+    raw_inputs = np.zeros((end-start, nb_in))
+    commands = np.zeros((end-start, nb_out))
+    raw_commands = np.zeros((end-start, nb_out))
+    for el in conf['data']:
+        t = el['type']
+        idx = el['index']
+        col = el['column']
+        if t == 'input' and idx >= 0:
+            if idx >= nb_in:
+                print("Invalid input index for {}".format(el['name']))
+                exit(1)
+            raw_inputs[:, idx] = apply_format(el, data[start:end, col])
+            inputs[:, idx] = apply_format(el, data[start:end, col])
+            for (filt_name, params) in el['filters']:
+                inputs[:,idx] = apply_filter(filt_name, params, inputs[:, idx].copy(), var)
+        if t == 'command' and idx >= 0:
+            if idx >= nb_out:
+                print("Invalid command index for {}".format(el['name']))
+                exit(1)
+            raw_commands[:, idx] = apply_format(el, data[start:end, col])
+            commands[:, idx] = apply_format(el, data[start:end, col])
+            for (filt_name, params) in el['filters']:
+                commands[:,idx] = apply_filter(filt_name, params, commands[:, idx].copy(), var)
+    return inputs, raw_inputs, commands, raw_commands
+
+
+
 #
 # Display functions
 #
 
-def plot_results(x, y, t, start, end, label, show=False):
+def plot_results(x, y, y_raw, z, t, freq, label, show=False):
     '''
     plot two curves for comparison
     '''
-    #print(np.shape(x), np.shape(y), np.shape(t))
-    plt.figure()
-    plt.plot(t, y)
-    plt.plot(t, x)
-    plt.xlabel('t [s]')
-    plt.ylabel(label)
-    plt.figure()
-    plt.plot(x[start:end], y[start:end])
-    plt.xlabel('command [pprz]')
-    plt.ylabel(label)
+    fig = plt.figure(layout='constrained')
+    ax_time = plt.subplot2grid((2,3), (0,0), colspan=2, rowspan=2)
+    ax_fit = plt.subplot2grid((2,3), (0,2))
+    ax_fft = plt.subplot2grid((2,3), (1,2))
+
+    # time plot
+    ax_time.plot(t, y)
+    ax_time.plot(t, x)
+    ax_time.set_title(label)
+    ax_time.set_xlabel('t [s]')
+
+    # Fit line
+    ax_fit.plot(x, y)
+    p = np.poly1d(z)
+    xp = np.linspace(np.min(x), np.max(x), 2)
+    ax_fit.plot(xp,p(xp),'r')
+    ax_fit.set_title(f'fit error: {abs(1.-z[0]):.3E}')
+    
+    # FFT
+    N = len(y)
+    T = 1./freq
+    yf = fft(y_raw)
+    xf = np.linspace(0.0, 1.0/(2.0*T), int(N/2))
+    ax_fft.plot(xf, 2.0/N * np.abs(yf[0:int(N/2)]))
+    ax_fft.grid()
+    ax_fft.set_xlabel('Freq (Hz)')
+    ax_fft.set_yticks([])
+    ax_fft.set_ylabel('FFT Amplitude')
+
     if show:
         plt.show()
 
-def print_results():
-    pass
+def plot_residuals(values, residuals, label, show=False):
+    plt.figure()
+    plt.plot(values, residuals)
+    plt.xlabel(label)
+    plt.ylabel('residual')
+    plt.grid()
+    plt.title(label)
+    if show:
+        plt.show()
+
+def value_or_default(key, dic, default):
+    if key in dic:
+        return dic[key]
+    else:
+        return default
+
+def print_results(conf, var, output):
+    if 'display' not in conf:
+        print("No display section")
+        return
+    
+    # get display info
+    disp = conf['display']
+
+    print("\nAdd the following lines to your airframe file:\n")
+    for d in disp:
+        name = d['name']
+        coef = value_or_default('coef', d, None)
+        matrix = value_or_default('matrix', d, None)
+        scaling = value_or_default('scaling', d, 1.)
+        if matrix is not None:
+            print(f'<define name="{name}" type="matrix">')
+            for i in range(len(matrix)):
+                l = []
+                for e in matrix[i]:
+                    if isinstance(e, (int, float, str)):
+                        l.append("{:.2f}".format(scaling*get_param(e, var)))
+                    else:
+                        l.append("{:.2f}".format(scaling*output[e[0], e[1]]))
+                print(f'  <field value="{", ".join(l)}" type="float[]"/>')
+            print('</define>')
+        elif coef is not None:
+            if isinstance(coef, (int, float, str)):
+                print('<define name="{}" value="{}"/>'.format(name, scaling*get_param(coef, var)))
+            elif len(coef) == 2 and isinstance(coef[0], int) and isinstance(coef[1], int):
+                print('<define name="{}" value="{:.2f}"/>'.format(name, scaling*output[coef[0], coef[1]]))
+            else:
+                l = []
+                for e in coef:
+                    if isinstance(e, (float, str)):
+                        l.append("{:.2f}".format(scaling*get_param(e, var)))
+                    elif isinstance(e, int):
+                        l.append("{}".format(int(scaling*get_param(e, var))))
+                    else:
+                        l.append("{:.2f}".format(scaling*output[e[0], e[1]]))
+                print('<define name="{}" value="{{{}}}"/>'.format(name, ', '.join(l)))
+
 
 
 #
 # Optimization functions
 #
 
-def fit_axis(x, y, axis, start, end, verbose=False):
-    c = np.linalg.lstsq(x[start:end], y[start:end], rcond=None)
+def fit_axis(x, y, name, verbose=False):
+    c = np.linalg.lstsq(x, y, rcond=None)
     if verbose:
-        print("Fit axis", axis)
+        print("Fit axis", name)
         print(c[0]*1000)
     return c[0]
 
+def fit_lin(x, y, name, verbose=False):
+    z = np.polyfit(x, y, 1, full=True)
+    if verbose:
+        print(f'Fit residual {name}: {float(z[1]):.5E}')
+    return z[0], z[1]