├── LICENSE ├── install.sh ├── README.md └── px4_pid_tuner.py /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Mohamed Abdelkader Zahana 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /install.sh: -------------------------------------------------------------------------------- 1 | 2 | # upgrading python 3.5 to 3.6 (to be able to use harold to get cont tf fro discrete) 3 | sudo add-apt-repository ppa:jonathonf/python-3.6 4 | 5 | sudo apt-get update 6 | 7 | sudo apt-get install python3.6 8 | 9 | sudo update-alternatives --install /usr/bin/python3 python3 /usr/bin/python3.6 1 10 | sudo apt-get install python3.6-dev 11 | 12 | # Needed for px4tools 13 | sudo apt-get install libgeos-3.5.0 14 | sudo apt-get install libgeos-dev 15 | sudo pip install https://github.com/matplotlib/basemap/archive/master.zip 16 | 17 | pip3 install numpy 18 | pip3 install px4tools 19 | 20 | pip3 install harold 21 | 22 | # Dependencies for SIPPY 23 | pip3 install control 24 | pip3 install scipy 25 | # needed for slycot 26 | pip3 install scikit-build 27 | pip3 install slycot 28 | pip3 install future 29 | pip3 install matplotlib 30 | # Needed to be able to "show" plots by matplotlib 31 | sudo apt-get install python3-tk 32 | 33 | # Install SIPPY from github 34 | git clone https://github.com/CPCLAB-UNIPI/SIPPY.git 35 | cd SIPPY 36 | python3 setup.py install 37 | # or, in case of permission error 38 | sudo python setup.py install 39 | 40 | # Genetic algorithm package 41 | pip install deap 42 | # or sudo pip install deap 43 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # px4_pid_tuner 2 | Python script for system identification and tuning of PX4 PID loops based on PX4 logs (ulog only). 3 | 4 | It currently tunes the attitude rate loops only, `ROLL_RATE_P`/I/D gains. Similarly, for pitch/yaw. Future updates will allow attitude loop P gain tuning as well as translational velocity and position loops. 5 | 6 | # Background 7 | The python script performs two main tasks. 8 | 1. Identifies a **2nd order system** which will be used for PID tuning. This is done using [SIPPY](https://github.com/CPCLAB-UNIPI/SIPPY) package. 9 | 2. Given the model identified in 1, it performs LQR-based PID tuning as described in [this paper](https://ieeexplore.ieee.org/document/4242954). In LQR-based tuning, the PID gains are optimal given specific LQR weight matrices, Q and R. To find best Q and R matrices, genetic optimization is employed using [DEAP](https://deap.readthedocs.io/en/master/overview.html) python package 10 | 11 | # Installation 12 | Look at the required modules in the `install.sh` file. 13 | 14 | # Usage 15 | The script is called from the command line with positional arguments as follows. 16 | * To only show input/output data for inspection before identification, you can use the `-sd true` or `--showDataOnly true` argument. 17 | ``` 18 | python3 px4_pid_tuner.py -f log.ulg -sd true 19 | ``` 20 | 21 | * To select which axis i.e. roll/pitch/yaw, use the `-a` or `--axis` 22 | ``` 23 | python3 px4_pid_tuner.py -f log.ulg -a roll -sd true 24 | ``` 25 | If not provided, the default is roll 26 | 27 | * To select data range (start/end time in seconds), use the `-s` for start and `-e` for end times. The time is in seconds 28 | ``` 29 | python3 px4_pid_tuner.py -f log.ulg -s 2.0 -e 5.5 -sd true 30 | ``` 31 | 32 | * If `-sd true` is not set, system identificaion and PID tuning will be performed. In addition, the data will be divided into mini-batches to find a data range with best-fit model. To disable the automatic data range selection, set autoSelect `-as false` and provide range of data manually using `-s ` and `-e `. For example, 33 | ``` 34 | python3 px4_pid_tuner.py -f log.ulg -as false -s 2.0 -e 5.5 35 | ``` 36 | 37 | * If you want the script to try to automatically select data range with best fit, set `-as true` and determine the batch size using `-bs 4.0` (4 seconds in this case). 38 | ``` 39 | python3 px4_pid_tuner.py -f log.ulg -as true -bs 4.0 40 | ``` 41 | 42 | * To determine the resampling time of i/o data, use `-dt 0.001`. Sampling time is in seconds. 43 | ``` 44 | python3 px4_pid_tuner.py -f log.ulg -as false -s 2.0 -e 5.5 -ddt 0.001 45 | ``` 46 | 47 | # Notes 48 | 49 | * The quality of system identification is highly dependent on the input/output data coming from the `ulg` file. If the system has poor data i.e. no excitation in different axes with different frequencies, the model can be off and PID tuning results will be useless. 50 | * Make sure that you use the good portions of your data (ones with enough excitation), and one where the controller is active (outputing signal). 51 | * The automatics data range selector (`-as true`) is not yet optimized, so, use common sense to judge if the resulting batch of the automatic range selector is really good portion of the data. 52 | 53 | # TODO 54 | - [ ] Do more data pre-procssing to get better identified system. 55 | - [ ] Implement a function similar to MATLAB function `advice()` (https://www.mathworks.com/help/ident/ref/advice.html). This will help to automatically analyze I/O data and provide recommendations for better model identification. 56 | - [ ] Allow user to graphically select range of data from a plot for system identification 57 | - [ ] Implement online optimal PID tuning ! -------------------------------------------------------------------------------- /px4_pid_tuner.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """ 3 | Python script for system identification and PID tuning of PX4 PID loops using PX4 logs (ulog only). 4 | @author Mohamed Abdelkader 5 | @email mohamedashraf123@gmail.com 6 | @year 2019 7 | 8 | USE IT AT YOUR OWN RISK! 9 | """ 10 | 11 | # Argument parser 12 | import argparse 13 | 14 | # Mainly for numpy arrays... 15 | import numpy as np 16 | 17 | # Need FFT ? 18 | import numpy.fft as fft 19 | 20 | # For optimization 21 | import scipy.optimize 22 | import scipy.signal 23 | 24 | # Read ulog files and prepare data 25 | import px4tools as px 26 | from collections import OrderedDict 27 | import pandas as pd 28 | from past.utils import old_div 29 | 30 | # System identification 31 | from sippy import * 32 | from sippy import functionset as fset 33 | from sippy import functionsetSIM as fsetSIM 34 | 35 | # Control design 36 | import control 37 | import control.matlab as cnt 38 | 39 | # To convert discrete-time transfer function to continuous-time 40 | import harold 41 | 42 | # For genetic optimization 43 | from deap import base 44 | from deap import creator 45 | from deap import tools 46 | import random 47 | 48 | # Plotting 49 | import matplotlib.pyplot as plt 50 | 51 | 52 | ########################################################### 53 | """ 54 | Data Preparation class 55 | """ 56 | class PrepData: 57 | def __init__(self, file=None, data_start_ind = 0, data_end_ind = -1, rolling_window_u = 100, rolling_window_y = 100, Ts = 0.001, data='roll', plot=True): 58 | """ 59 | Constructor 60 | @file String of path to ulog file 61 | @dt desired sampling time 62 | @data_start_ind Starting index of data 63 | @data_end_ind Last index of data. -1 means up to last element 64 | @data type of data required {'roll', 'pitch', 'yaw'} 65 | @rolling_window moving average window length 66 | @Ts resampling times in seconds 67 | """ 68 | 69 | self._raw_data = None 70 | if file is None: 71 | print("Path to ulg file is not provided") 72 | return 73 | 74 | self._raw_data = px.read_ulog(file) 75 | 76 | self._dt = Ts # will be calculated from data time index, see process_data() 77 | self._Ts = Ts 78 | 79 | self._full_data_length = None # after resampling 80 | 81 | self._rolling_window_u = rolling_window_u # moving average window length for controller output filtering 82 | self._rolling_window_y = rolling_window_y # moving average window length for plant output filtering 83 | 84 | self._data_start_ind = data_start_ind 85 | self._data_end_ind = data_end_ind 86 | 87 | data_str = ['roll', 'pitch', 'yaw'] 88 | if data not in data_str: 89 | print("data is not among ['roll', 'pitch', 'yaw']. Defaulting to 'roll' ") 90 | data = 'roll' 91 | 92 | self._data_name = data 93 | 94 | # Returned variables 95 | self._t = None # time vector 96 | self._u_raw = None # plant input data 97 | self._y_raw = None # plant output data 98 | self._u_filtered = None 99 | self._y_filtered = None 100 | 101 | self._trimmed_raw_data = None # between _data_start_ind and _data_end_ind 102 | 103 | self._plot = plot 104 | 105 | def process_data(self): 106 | """ 107 | Returns processed data (time t,input u, output y) for the selected data type ['roll', 'pitch', 'yaw'] 108 | Current processing: 109 | * resampling (mostly upsampling) to get rid of certain noise frequency 110 | * rolling window (moving) average 111 | 112 | TODO: make some analysis to advise if data is suitable for sysid. Something similar to MATLAB function advice() 113 | https://www.mathworks.com/help/ident/ref/advice.html 114 | """ 115 | 116 | if self._raw_data is None: 117 | print("No data available!") 118 | return 119 | 120 | print('Processing data for '+self._data_name) 121 | 122 | # Extract topics of interest and make data the same length as the controller output t_actuator_controls_0_0 123 | d_concat = self._raw_data.concat( topics=['t_actuator_controls_0_0','t_vehicle_attitude_0'],on="t_actuator_controls_0_0") 124 | 125 | # Find _dt from one of t_actuator_controls_0_0 fields e.g. t_actuator_controls_0_0__f_control_0_ 126 | #t = (d_concat['t_actuator_controls_0_0__f_control_0_'].index.total_seconds() * 1e3).values 127 | #t_diff = np.diff(t) 128 | #dt_ms = int(t_diff.sum()/len(t_diff)) 129 | #print("dt_ms", dt_ms) 130 | 131 | # or compute dt from user input (_Ts) 132 | dt_ms = int(self._Ts *1000) 133 | dt_ms_str = str(dt_ms)+'L' 134 | 135 | self._dt= dt_ms/1000. 136 | 137 | # Resample e with dt_ms 138 | df_rs = d_concat.resample(dt_ms_str).mean().interpolate() #method='spline', order=2s 139 | df_rs.index = [i * self._dt for i in range(len(df_rs.index))] 140 | self._full_data_length = len(df_rs.index) 141 | 142 | self._trimmed_raw_data = df_rs[['t_actuator_controls_0_0__f_control_0_','t_vehicle_attitude_0__f_rollspeed', 143 | 't_actuator_controls_0_0__f_control_1_','t_vehicle_attitude_0__f_pitchspeed', 144 | 't_actuator_controls_0_0__f_control_2_','t_vehicle_attitude_0__f_yawspeed']] 145 | 146 | self._trimmed_raw_data.rename(columns={'t_actuator_controls_0_0__f_control_0_':'ATTC_Roll', 147 | 't_actuator_controls_0_0__f_control_1_':'ATTC_Pitch', 148 | 't_actuator_controls_0_0__f_control_2_':'ATTC_Yaw', 149 | 't_vehicle_attitude_0__f_rollspeed':'rollspeed', 150 | 't_vehicle_attitude_0__f_pitchspeed':'pitchspeed', 151 | 't_vehicle_attitude_0__f_yawspeed':'yawspeed'}, 152 | inplace=True) 153 | 154 | self._full_data_length = len(df_rs.index) 155 | # Time steps 156 | Time = np.array([i * self._dt for i in range(self._full_data_length)]) 157 | Time = np.expand_dims(Time, axis=0) 158 | 159 | # Rate controller output is stored in the following fields 160 | # roll_rate control output: t_actuator_controls_0_0__f_control_0_ 161 | # pitch_rate control output: t_actuator_controls_0_0__f_control_1_ 162 | # yaw_rate control output: t_actuator_controls_0_0__f_control_2_ 163 | 164 | if self._data_name == 'roll': 165 | data_str_u = 't_actuator_controls_0_0__f_control_0_' 166 | data_str_y = 't_vehicle_attitude_0__f_rollspeed' 167 | elif self._data_name == 'pitch': 168 | data_str_u = 't_actuator_controls_0_0__f_control_1_' 169 | data_str_y = 't_vehicle_attitude_0__f_pitchspeed' 170 | elif self._data_name == 'yaw': 171 | data_str_u = 't_actuator_controls_0_0__f_control_2_' 172 | data_str_y = 't_vehicle_attitude_0__f_yawspeed' 173 | 174 | raw_u = df_rs[data_str_u] # rate controller output 175 | raw_u_v = raw_u.values # ndarray 176 | raw_u_bias = raw_u.rolling(self._rolling_window_u, min_periods=1,center=True).mean() # min_periods=1 used to avoid NaNs in the first elements 177 | #raw_u_filtered = raw_u - raw_u_bias 178 | raw_u_filtered = raw_u_bias 179 | raw_u_filtered_v = raw_u_filtered.values # ndarray 180 | raw_u_filtered_v = np.expand_dims(raw_u_filtered_v, axis=0) 181 | raw_u_v = np.expand_dims(raw_u_v, axis=0) 182 | 183 | #Feedback signals 184 | # roll speed: t_vehicle_attitude_0__f_rollspeed 185 | # pitch speed: t_vehicle_attitude_0__f_pitchspeed 186 | # yaw speed: t_vehicle_attitude_0__f_yawspeed 187 | 188 | raw_feedback = df_rs[data_str_y] 189 | raw_feedback_v = raw_feedback.values # ndarray 190 | #raw_feedback_acc = raw_feedback.diff().fillna(df_rs[data_str_y])/ self._dt 191 | raw_feedback_bias = raw_feedback.rolling(self._rolling_window_y, min_periods=1,center=True).mean() 192 | #raw_feedback_filtered = raw_feedback - raw_feedback_bias 193 | raw_feedback_filtered = raw_feedback_bias 194 | raw_feedback_filtered_v = raw_feedback_filtered.values # ndarray 195 | raw_feedback_filtered_v = np.expand_dims(raw_feedback_filtered_v, axis=0) 196 | raw_feedback_v = np.expand_dims(raw_feedback_v, axis=0) 197 | 198 | 199 | # Sanity checks 200 | if self._data_end_ind > self._full_data_length or self._data_end_ind < 0 : 201 | self._data_end_ind = self._full_data_length 202 | 203 | if self._data_start_ind > self._full_data_length or self._data_end_ind < 0: 204 | self._data_start_ind = 0 205 | 206 | # Time 207 | self._t = Time #[:,self._data_start_ind:self._data_end_ind] 208 | 209 | # Plant raw I/O 210 | self._u_raw = raw_u_v #[:,self._data_start_ind:self._data_end_ind] 211 | self._y_raw = raw_feedback_v #[:,self._data_start_ind:self._data_end_ind] 212 | # Plant filtered I/O 213 | self._u_filtered = raw_u_filtered_v #[:,self._data_start_ind:self._data_end_ind] 214 | self._y_filtered = raw_feedback_filtered_v #[:,self._data_start_ind:self._data_end_ind] 215 | 216 | if self._plot: 217 | ax1 = plt.subplot(2, 2, 1) 218 | plt.plot(self._t[0,self._data_start_ind:self._data_end_ind], self._y_raw[0,self._data_start_ind:self._data_end_ind]) 219 | plt.plot(self._t[0,self._data_start_ind:self._data_end_ind], self._y_filtered[0,self._data_start_ind:self._data_end_ind]) 220 | plt.xlabel("Time[s]") 221 | plt.ylabel(self._data_name+" speed feedback [rad/s]") 222 | plt.title(self._data_name+' speed feedback: Raw vs. Filtered') 223 | plt.legend(['Raw', 'Filtered']) 224 | plt.grid(True) 225 | 226 | plt.subplot(2, 2, 2, sharex=ax1) 227 | plt.plot(self._t[0,self._data_start_ind:self._data_end_ind], self._u_raw[0,self._data_start_ind:self._data_end_ind]) 228 | plt.plot(self._t[0,self._data_start_ind:self._data_end_ind], self._u_filtered[0,self._data_start_ind:self._data_end_ind]) 229 | plt.xlabel("Time[s]") 230 | plt.ylabel(self._data_name+" speed control [rad/s]") 231 | plt.title(self._data_name+' rate controller Output: Raw vs. Filtered') 232 | plt.legend(['Raw', 'Filtered']) 233 | plt.grid(True) 234 | 235 | plt.subplot(2, 2, 3, sharex=ax1) 236 | plt.plot(self._t[0,self._data_start_ind:self._data_end_ind], self._y_raw[0,self._data_start_ind:self._data_end_ind]) 237 | plt.plot(self._t[0,self._data_start_ind:self._data_end_ind], self._u_raw[0,self._data_start_ind:self._data_end_ind]) 238 | plt.xlabel("Time[s]") 239 | plt.ylabel(self._data_name+" rate [rad/s]") 240 | plt.title(self._data_name+' rate raw input and output') 241 | plt.legend(['Output', 'Input']) 242 | plt.grid(True) 243 | 244 | plt.subplot(2, 2, 4, sharex=ax1) 245 | plt.plot(self._t[0,self._data_start_ind:self._data_end_ind], self._y_filtered[0,self._data_start_ind:self._data_end_ind]) 246 | plt.plot(self._t[0,self._data_start_ind:self._data_end_ind], self._u_filtered[0,self._data_start_ind:self._data_end_ind]) 247 | plt.xlabel("Time[s]") 248 | plt.ylabel(self._data_name+" rate [rad/s]") 249 | plt.title(self._data_name+' rate filtered input and output') 250 | plt.legend(['Output', 'Input']) 251 | plt.grid(True) 252 | 253 | 254 | plt.show() 255 | 256 | def get_data(self): 257 | if self._raw_data is None: 258 | print("Run process_data() first!") 259 | 260 | # return self._trimmed_raw_data.iloc[self._data_start_ind : self._data_end_ind, :] 261 | 262 | return self._full_data_length, self._t[:,self._data_start_ind:self._data_end_ind], self._u_filtered[:,self._data_start_ind:self._data_end_ind], self._y_filtered[:,self._data_start_ind:self._data_end_ind], self._dt 263 | 264 | def set_param(self, start_ind = 0, end_ind=-1): 265 | self._data_start_ind = start_ind 266 | self._data_end_ind = end_ind 267 | 268 | 269 | 270 | ########################################################### 271 | """ 272 | System identification class 273 | """ 274 | class PX4SYSID: 275 | """ 276 | Uses SIPPY package to do sys ID and provide a 2nd order system in continuous time domain 277 | """ 278 | def __init__(self, t, u, y, use_subspace =True, subspace_method='N4SID', subspace_p=250, Ts=0.001, u_name='input', y_name='ouput', plot = True, verbose = False): 279 | """ 280 | @t: array of time samples [seconds] 281 | @u input array 282 | @y output array 283 | @use_subspace: Uses N4SID if True, otherwises it uses ARMAX 284 | @subspace_method subspace method in case use_subspace=True. Methods are N4SID, CVA, MOESP 285 | @subspace_p see SIPPY docs for explanation 286 | @Ts: Data sampling time 287 | """ 288 | self._use_subspace = use_subspace 289 | self._subspace_p = subspace_p 290 | self._subspace_method = subspace_method 291 | self._Ts = Ts 292 | 293 | self._u = u 294 | self._y = y 295 | self._t = t # time vector 296 | 297 | self._u_name = u_name 298 | self._y_name = y_name 299 | 300 | # Reuturned identified system 301 | self._A, self._B, self._C, self._D = None, None, None, None # state space 302 | self._sys_tf = None # transfer function 303 | self._zeros = None 304 | self._fitness = 0. # How much the id system fits the actual output 305 | 306 | 307 | self._plot = plot 308 | self._verbose = verbose 309 | 310 | self.do_sys_id() 311 | 312 | def do_sys_id(self): 313 | num_poles = 2 314 | num_zeros = 1 315 | if not self._use_subspace: 316 | method = 'ARMAX' 317 | #sys_id = system_identification(self._y.T, self._u[0], method, IC='BIC', na_ord=[0, 5], nb_ord=[1, 5], nc_ord=[0, 5], delays=[0, 5], ARMAX_max_iterations=300, tsample=self._Ts, centering='MeanVal') 318 | sys_id = system_identification(self._y.T, self._u[0], method, ARMAX_orders=[num_poles,1,1,0],ARMAX_max_iterations=300, tsample=self._Ts, centering='MeanVal') 319 | 320 | print(sys_id.G) 321 | 322 | if self._verbose: 323 | print(sys_id.G) 324 | print("System poles of discrete G: ",cnt.pole(sys_id.G)) 325 | 326 | 327 | # Convert to continuous tf 328 | G = harold.Transfer(sys_id.NUMERATOR,sys_id.DENOMINATOR, dt=self._Ts) 329 | G_cont = harold.undiscretize(G, method='zoh') 330 | self._sys_tf = G_cont 331 | self._A, self._B, self._C, self._D = harold.transfer_to_state(G_cont, output='matrices') 332 | 333 | if self._verbose: 334 | print("Continuous tf:" , G_cont) 335 | 336 | 337 | # Convert to state space, because ARMAX gives transfer function 338 | ss_roll = cnt.tf2ss(sys_id.G) 339 | A = np.asarray(ss_roll.A) 340 | B = np.asarray(ss_roll.B) 341 | C = np.asarray(ss_roll.C) 342 | D = np.asarray(ss_roll.D) 343 | if self._verbose: 344 | print(ss_roll) 345 | 346 | # simulate identified system using input from data 347 | xid, yid = fsetSIM.SS_lsim_process_form(A, B, C, D, self._u) 348 | y_error = self._y - yid 349 | self._fitness = 1 - (y_error.var()/self._y.var())**2 350 | if self._verbose: 351 | print("Fittness %", self._fitness*100) 352 | 353 | if self._plot: 354 | plt.figure(1) 355 | plt.plot(self._t[0], self._y[0]) 356 | plt.plot(self._t[0],yid[0]) 357 | plt.xlabel("Time") 358 | plt.title("Time response Y(t)=U*G(t)") 359 | plt.legend([self._y_name, self._y_name+'_identified: '+'{:.3f} fitness'.format(self._fitness)]) 360 | plt.grid() 361 | plt.show() 362 | 363 | else: 364 | sys_id = system_identification(self._y, self._u, self._subspace_method, SS_fixed_order=num_poles, SS_p=self._subspace_p, SS_f=50, tsample=self._Ts, SS_A_stability=True, centering='MeanVal') 365 | #sys_id = system_identification(self._y, self._u, self._subspace_method, SS_orders=[1,10], SS_p=self._subspace_p, SS_f=50, tsample=self._Ts, SS_A_stability=True, centering='MeanVal') 366 | if self._verbose: 367 | print("x0", sys_id.x0) 368 | print("A",sys_id.A) 369 | print("B",sys_id.B) 370 | print("C",sys_id.C) 371 | print("D",sys_id.D) 372 | 373 | A = sys_id.A 374 | B = sys_id.B 375 | C = sys_id.C 376 | D = sys_id.D 377 | 378 | # Get discrete transfer function from state space 379 | sys_tf = cnt.ss2tf(A, B, C, D) 380 | if self._verbose: 381 | print("TF ***in z domain***", sys_tf) 382 | 383 | # Get numerator and denominator 384 | (num,den) = cnt.tfdata(sys_tf) 385 | 386 | # Convert to continuous tf 387 | G = harold.Transfer(num,den, dt=self._Ts) 388 | if self._verbose: 389 | print(G) 390 | G_cont = harold.undiscretize(G, method='zoh') 391 | self._sys_tf = G_cont 392 | self._A, self._B, self._C, self._D = harold.transfer_to_state(G_cont, output='matrices') 393 | if self._verbose: 394 | print("Continuous tf:" , G_cont) 395 | 396 | # get zeros 397 | tmp_tf = cnt.ss2tf(self._A, self._B, self._C, self._D) 398 | self._zeros = cnt.zero(tmp_tf) 399 | 400 | # simulate identified system using discrete system 401 | xid, yid = fsetSIM.SS_lsim_process_form(A, B, C, D, self._u, sys_id.x0) 402 | y_error = self._y - yid 403 | self._fitness = 1 - (y_error.var()/self._y.var())**2 404 | if self._verbose: 405 | print("Fittness %", self._fitness*100) 406 | 407 | if self._plot: 408 | plt.figure(1) 409 | plt.plot(self._t[0], self._y[0]) 410 | plt.plot(self._t[0],yid[0]) 411 | plt.xlabel("Time") 412 | plt.title("Time response Y(t)=U*G(t)") 413 | plt.legend([self._y_name, self._y_name+'_identified: '+'{:.3f} fitness'.format(self._fitness)]) 414 | plt.grid() 415 | plt.show() 416 | 417 | def get_data(self): 418 | return self._A, self._B, self._C, self._D, self._sys_tf, self._fitness, self._zeros 419 | 420 | 421 | ########################################################### 422 | """ 423 | LQR-based PID design class 424 | """ 425 | class PX4PIDDesign: 426 | """ 427 | Class for LQR-based PID design. Currently, for a 2nd order system only 428 | Reference: https://ieeexplore.ieee.org/document/4242954 429 | """ 430 | def __init__(self, A, B, C, D, q=[1., 1. , 1.], r = 1.0, dt=0.001 , verobse=False): 431 | self._A = A 432 | self._B = B 433 | self._C = C 434 | self._D = D 435 | 436 | # LQR weights 437 | self._q = q 438 | self._r = r 439 | 440 | # Calculated gains 441 | self._K = None 442 | 443 | self._dt = dt 444 | 445 | self._verbose = verobse 446 | 447 | def get_gains(self): 448 | if self._K is None: 449 | print("Gains are not computed yet.") 450 | 451 | return self._K 452 | 453 | 454 | def set_weights(self, q=[1.0, 1.0, 1.0], r=1.0): 455 | self._q = q 456 | self._r = r 457 | 458 | def pid_design(self): 459 | 460 | n_x = self._A.shape[0] # number of sates 461 | n_u = self._B.shape[1] # number of inputs 462 | n_y = self._C.shape[0] # number of outputs 463 | 464 | # Augmented state system (for LQR) 465 | A_lqr = np.block([ [ self._A , np.zeros( (n_x,n_y) )], 466 | [self._C, np.zeros((n_y,n_y))] 467 | ]) 468 | B_lqr = np.block([ 469 | [self._B], 470 | [np.zeros((n_y,1))] 471 | ]) 472 | 473 | # Define Q,R 474 | Q = np.diag(self._q) 475 | R = np.array([self._r]) 476 | # Solve for P in continuous-time algebraic Riccati equation (CARE) 477 | #print("A_lqr shape", A_lqr.shape) 478 | #print("Q shape",Q.shape) 479 | (P,L,F) = cnt.care(A_lqr, B_lqr, Q, R) 480 | 481 | if self._verbose: 482 | print("P matrix", P) 483 | print("Feedback gain", F) 484 | 485 | # Calculate Ki_bar, Kp_bar 486 | Kp_bar = np.array( [ F[0][0:n_x] ] ) 487 | Ki_bar = np.array( [ F[0][n_x:] ] ) 488 | 489 | if self._verbose: 490 | print("Kp_bar", Kp_bar) 491 | print("Ki_bar", Ki_bar) 492 | 493 | # Calculate the PID kp kd gains 494 | C_bar = np.block([ 495 | [ self._C ], 496 | [ self._C.dot(self._A) - (self._C.dot(self._B)).dot(Kp_bar) ] 497 | ]) 498 | 499 | if self._verbose: 500 | print("C_bar", C_bar) 501 | print("C_bar shape", C_bar.shape) 502 | 503 | # Calculate PID kp ki gains 504 | kpd = Kp_bar.dot(np.linalg.inv(C_bar)) 505 | if self._verbose: 506 | print("kpd: ",kpd, "with shape: ", kpd.shape) 507 | kp = kpd[0][0] 508 | kd = kpd[0][1] 509 | # ki gain 510 | ki = (1. + kd* self._C.dot(self._B)).dot(Ki_bar) 511 | self._K = [kp, ki[0][0], kd] 512 | 513 | G_plant = cnt.ss2tf(self._A, self._B, self._C, self._D) 514 | 515 | # create PID transfer function 516 | d_tc = 1./125. # Derivative time constant at nyquist freq 517 | p_tf = self._K[0] 518 | i_tf = self._K[1]*cnt.tf([1],[1,0]) 519 | d_tf = self._K[2]*cnt.tf([1,0],[d_tc,1]) 520 | pid_tf = cnt.parallel(p_tf, i_tf, d_tf) 521 | open_loop_tf = cnt.series(pid_tf, G_plant) 522 | closedLoop_tf = cnt.feedback(open_loop_tf,1) 523 | 524 | if self._verbose: 525 | print(" *********** PID gains ***********") 526 | print("kp: ", self._K[0] ) 527 | print("ki: ", self._K[1]) 528 | print("kd: ", self._K[2]) 529 | print(" *********************************") 530 | 531 | return pid_tf, closedLoop_tf 532 | 533 | def step_response(self): 534 | 535 | if self._verbose: 536 | print("[PID Design] Calculating PID gains") 537 | pid_tf, closedLoop_tf = self.pid_design() 538 | 539 | 540 | 541 | end_time = 5 # [s] 542 | npts = int(old_div(end_time, self._dt)) + 1 543 | T = np.linspace(0, end_time, npts) 544 | yout, T = cnt.step(closedLoop_tf, T) 545 | 546 | u, _, _ = cnt.lsim(pid_tf,1-yout,T) 547 | 548 | return T, u, yout 549 | 550 | 551 | def plotStepResponse(self): 552 | """ 553 | Plots the step response of the closed loop, using gains in K. 554 | """ 555 | 556 | T, u, yout = self.step_response() 557 | 558 | fig, ((ax1),(ax2)) = plt.subplots(2,1) 559 | ax1.plot(T, yout) 560 | ax1.set_title('Step Response') 561 | ax2.plot(T, u) 562 | ax2.set_title('Controller Output') 563 | plt.show() 564 | 565 | 566 | ########################################################### 567 | """ 568 | GA class 569 | """ 570 | class GeneticOptimization: 571 | def __init__(self, A,B,C,D, num_pop = 50, num_gen = 10, w1=0.5,w2=0.5, dt=0.001, verbose=False): 572 | """ 573 | @param G Plant transfer function 574 | @param dt Sampling time 575 | @param num_pop populaion size 576 | @param num_gen Maximum number of generations 577 | """ 578 | self._weight_min = 0.5 579 | self._weight_max = 1000.0 580 | 581 | self._w1 = w1 582 | self._w2 = w2 583 | 584 | self._dt = dt 585 | 586 | self._num_pop = num_pop 587 | self._num_gen = num_gen 588 | 589 | # Best solution (computed) 590 | self._best_sol = None 591 | 592 | self._verbose = verbose 593 | 594 | self._pid = PX4PIDDesign(A, B, C, D, dt=dt, verobse=False) 595 | 596 | def weight_value(self): 597 | """ 598 | @return K: random values of weights within gain bounds 599 | """ 600 | return random.uniform(self._weight_min, self._weight_max) 601 | 602 | def run_ga(self): 603 | creator.create("FitnessMin", base.Fitness, weights=(-1.0,)) 604 | creator.create("Individual", list, fitness=creator.FitnessMin) 605 | 606 | toolbox = base.Toolbox() 607 | 608 | # Weight generator 609 | # define 'gain_value' to be an attribute ('gene') 610 | # which corresponds to non-negative real numbers sampled uniformly 611 | # from the range [0,100] 612 | toolbox.register("gain_value", self.weight_value) 613 | 614 | # Structure initializers 615 | # define 'individual' to be an individual 616 | # consisting of 4 'weight_value' elements ('genes') 617 | toolbox.register("individual", tools.initRepeat, creator.Individual, 618 | toolbox.gain_value, 4) 619 | 620 | # define the population to be a list of individuals 621 | toolbox.register("population", tools.initRepeat, list, toolbox.individual) 622 | 623 | #---------- 624 | # Operator registration 625 | #---------- 626 | # register the goal / fitness function 627 | toolbox.register("evaluate", self.evaluatePID) 628 | 629 | # register the crossover operator 630 | toolbox.register("mate", tools.cxTwoPoint) 631 | 632 | # register a mutation operator with a probability to 633 | # flip each attribute/gene of 0.05 634 | toolbox.register("mutate", tools.mutFlipBit, indpb=0.05) 635 | 636 | # operator for selecting individuals for breeding the next 637 | # generation: each individual of the current generation 638 | # is replaced by the 'fittest' (best) of three individuals 639 | # drawn randomly from the current generation. 640 | toolbox.register("select", tools.selTournament, tournsize=3) 641 | 642 | # create an initial population of 300 individuals (where 643 | # each individual is a list of integers) 644 | pop = toolbox.population(n=self._num_pop) 645 | 646 | # CXPB is the probability with which two individuals 647 | # are crossed 648 | # 649 | # MUTPB is the probability for mutating an individual 650 | CXPB, MUTPB = 0.5, 0.2 651 | 652 | print(" [GA] Start of evolution") 653 | 654 | # Evaluate the entire population 655 | fitnesses = list(map(toolbox.evaluate, pop)) 656 | for ind, fit in zip(pop, fitnesses): 657 | ind.fitness.values = fit 658 | if self._verbose: 659 | print(" [GA] Evaluated %i individuals" % len(pop)) 660 | 661 | # Extracting all the fitnesses of 662 | fits = [ind.fitness.values[0] for ind in pop] 663 | 664 | # Variable keeping track of the number of generations 665 | g = 0 666 | 667 | # Begin the evolution 668 | print(" [GA] evaluating {} generations".format(self._num_gen)) 669 | while g < self._num_gen: 670 | # A new generation 671 | g = g + 1 672 | print(" [GA] -------------------------- Generation %i --------------------------" % g) 673 | 674 | # Select the next generation individuals 675 | offspring = toolbox.select(pop, len(pop)) 676 | # Clone the selected individuals 677 | offspring = list(map(toolbox.clone, offspring)) 678 | 679 | # Apply crossover and mutation on the offspring 680 | for child1, child2 in zip(offspring[::2], offspring[1::2]): 681 | 682 | # cross two individuals with probability CXPB 683 | if random.random() < CXPB: 684 | toolbox.mate(child1, child2) 685 | 686 | # fitness values of the children 687 | # must be recalculated later 688 | del child1.fitness.values 689 | del child2.fitness.values 690 | 691 | for mutant in offspring: 692 | 693 | # mutate an individual with probability MUTPB 694 | if random.random() < MUTPB: 695 | toolbox.mutate(mutant) 696 | del mutant.fitness.values 697 | 698 | # Evaluate the individuals with an invalid fitness 699 | invalid_ind = [ind for ind in offspring if not ind.fitness.valid] 700 | fitnesses = map(toolbox.evaluate, invalid_ind) 701 | for ind, fit in zip(invalid_ind, fitnesses): 702 | ind.fitness.values = fit 703 | if self._verbose: 704 | print(" [GA] Evaluated %i individuals" % len(invalid_ind)) 705 | 706 | # The population is entirely replaced by the offspring 707 | pop[:] = offspring 708 | 709 | # Gather all the fitnesses in one list and print the stats 710 | fits = [ind.fitness.values[0] for ind in pop] 711 | 712 | length = len(pop) 713 | mean = sum(fits) / length 714 | sum2 = sum(x*x for x in fits) 715 | std = abs(sum2 / length - mean**2)**0.5 716 | 717 | if self._verbose: 718 | print(" [GA] Min %s" % min(fits)) 719 | print(" [GA] Max %s" % max(fits)) 720 | print(" [GA] Avg %s" % mean) 721 | print(" [GA] Std %s" % std) 722 | 723 | print(" [GA] -- End of (successful) evolution --") 724 | 725 | best_ind = tools.selBest(pop, 1)[0] 726 | self._best_sol = best_ind 727 | #print("Best individual is %s, %s" % (best_ind, best_ind.fitness.values)) 728 | q = [best_ind[0], best_ind[1], best_ind[2]] 729 | r = best_ind[3] 730 | print("optimal q", q) 731 | print("optimal r", r) 732 | 733 | return q, r 734 | 735 | def evaluatePID(self, W): 736 | """ 737 | Computes cost associated with system response (e.g. step response) of the closed loop, using LQR weights W. 738 | The cost to minimize is the time domain performance index defined in Equaiton 22 in 739 | # https://www.sciencedirect.com/science/article/pii/S0307904X12005197 740 | # sum_t(w_1 * t * err_t^2 + w_2 * u_t^2) * dt 741 | # err = setpoint_t - ouput_t 742 | 743 | @param W array of 4 LQR weights, W[0:3] = [q1, q2, q3]; W[3] = R 744 | @param G plant continuous-time transfer function 745 | @param Q scalar, state weight 746 | @param R scalar, input weight 747 | @param dt time step [sec] for control input discretization 748 | 749 | @return J cost >= 0 750 | """ 751 | 752 | q = [W[0], W[1], W[2]] 753 | r = W[3] 754 | # Simulate step response with PID 755 | self._pid.set_weights(q = q, r=r) 756 | try: 757 | self._pid.pid_design() 758 | t, u, yout = self._pid.step_response() 759 | 760 | J = self._dt*sum(t * self._w1 * (1-yout)**2 + self._w2 * u**2) 761 | except: 762 | J=1e9 763 | 764 | return J, 765 | 766 | def get_gains(self): 767 | if self._best_sol is None: 768 | print("Run optimization first!") 769 | 770 | q = [self._best_sol[0], self._best_sol[1], self._best_sol[2]] 771 | r = self._best_sol[3] 772 | # Simulate step response with PID 773 | self._pid.set_weights(q = q, r=r) 774 | self._pid.pid_design() 775 | 776 | return self._pid.get_gains() 777 | 778 | def main(args): 779 | 780 | AUTO_DATA_SEL = args["autoSelect"] 781 | 782 | ################## Prepare date ################## 783 | dt = float(args["samplingTime"]) 784 | start_t, end_t = float(args["start"]), float(args["end"]) 785 | start_i = int(start_t/dt) 786 | if end_t < 0.0: 787 | end_i = -1 788 | else: 789 | end_i = int(end_t/dt) 790 | data = PrepData( file=args["file"], data_start_ind = start_i, data_end_ind = end_i, rolling_window_y=50, rolling_window_u=50, Ts = dt, data=args["axis"], plot=True) 791 | data.process_data() 792 | full_data_len, t, u, y, dt = data.get_data() 793 | 794 | if args["showDataOnly"]: 795 | return 796 | 797 | if not AUTO_DATA_SEL: 798 | sysid = PX4SYSID(t, u, y, use_subspace =True, subspace_method='N4SID', Ts=dt, u_name='input', y_name='ouput', subspace_p=400, plot = True) 799 | best_A, best_B, best_C, best_D, tf, best_fit, zeros = sysid.get_data() 800 | print("Fitness", best_fit) 801 | print("Identified Transfer function: ", tf) 802 | else: 803 | # batch size 804 | batch_size_s = float(args["batchSize"]) # in seoconds 805 | batch_size = int(batch_size_s/dt) # in number of points 806 | 807 | if full_data_len < batch_size: 808 | print("Not enough data points. Get longer logs") 809 | return 810 | 811 | # find how many batches are available in full data 812 | n_batches = int(full_data_len/batch_size) 813 | print("Found {} batches".format(n_batches)) 814 | 815 | 816 | ################## SYS ID ################## 817 | best_fit = fit = 0.0 818 | best_A, best_B, best_C, best_D = None, None, None, None 819 | best_t, best_u, best_y = None, None, None 820 | best_zero = np.inf 821 | subspace_p_increment = 50 822 | n_intervals = (600 - subspace_p_increment)/subspace_p_increment 823 | for batch_i in range(n_batches): 824 | print("Processing batch# {}".format(batch_i)) 825 | 826 | start_i = batch_i * batch_size 827 | end_i = start_i + batch_size 828 | data.set_param(start_ind=start_i, end_ind=end_i) 829 | full_data_len, t, u, y, dt = data.get_data() 830 | best_p = 0 831 | for i in range(int(n_intervals)): 832 | p = (i+1)*subspace_p_increment 833 | try: 834 | sysid = PX4SYSID(t, u, y, use_subspace =True, subspace_method='N4SID', Ts=dt, u_name='input', y_name='ouput', subspace_p=p, plot = False) 835 | A,B,C,D,_, fit, zeros = sysid.get_data() 836 | except: 837 | print("Bad batch...... skipping") 838 | break 839 | 840 | if fit > best_fit and min(zeros) < 0.0 : # best_zero: 841 | best_A, best_B, best_C, best_D = A, B, C, D 842 | best_t, best_u, best_y = t, u, y 843 | best_fit = fit 844 | best_zero = min(zeros) 845 | best_p = p 846 | 847 | print("Best fit = {}".format(best_fit)) 848 | 849 | sysid = PX4SYSID(best_t, best_u, best_y, use_subspace =True, subspace_method='N4SID', Ts=dt, u_name='best_input', y_name='best_ouput', subspace_p=best_p, plot = True) 850 | _,_,_,_,tf, _, _ = sysid.get_data() 851 | print("Identified Transfer function: ", tf) 852 | 853 | if best_fit < 0.75 or np.isnan(best_fit): 854 | print(" [SYSID] WARNING Poor fitting %s. Try different data window with more dynamics!" %best_fit) 855 | print("Not proceeding with control tuning.") 856 | return 857 | 858 | ################## PID design ################## 859 | print(" --------------- Finding optimal gains using Genetic Optimization ---------------") 860 | ga = GeneticOptimization(best_A, best_B, best_C, best_D, num_pop = 25, num_gen = 10, w1=0.7, w2=0.3, dt=dt) 861 | q,r = ga.run_ga() 862 | 863 | print("optimal {} rate PID gains {}".format(args["axis"], ga.get_gains())) 864 | 865 | pid = PX4PIDDesign(best_A, best_B, best_C, best_D) 866 | pid.set_weights(q,r) 867 | pid.plotStepResponse() 868 | 869 | 870 | def str2bool(v): 871 | """ 872 | To be passed to argparse for parsing True/False commandline arguments 873 | """ 874 | if isinstance(v, bool): 875 | return v 876 | if v.lower() in ('yes', 'true', 't', 'y', '1'): 877 | return True 878 | elif v.lower() in ('no', 'false', 'f', 'n', '0'): 879 | return False 880 | else: 881 | raise argparse.ArgumentTypeError('Boolean value expected.') 882 | 883 | if __name__ == "__main__": 884 | # Parse command line arguments 885 | ap = argparse.ArgumentParser() 886 | 887 | ap.add_argument("-f", "--file", required=True, 888 | help="Path to ulog file.") 889 | ap.add_argument("-sd", "--showDataOnly", type=str2bool, required=False, 890 | help=" Show data only with no SYS ID or PID design. ", default=False) 891 | ap.add_argument("-a", "--axis", required=False, 892 | help=" 'roll', 'pitch', 'yaw' ", default="pitch") 893 | ap.add_argument("-as", "--autoSelect", type=str2bool, required=False, 894 | help=" Automatic selection of data batch for system identificaion ", default=True) 895 | ap.add_argument("-bs", "--batchSize", required=False, 896 | help=" Batch length in seconds ", default=5.0) 897 | ap.add_argument("-s", "--start", required=False, 898 | help=" Start time in seconds, if auto batch selection is false", default=1.0) 899 | ap.add_argument("-e", "--end", required=False, 900 | help=" End time in seconds, if auto batch selection is false", default=-1.0) 901 | ap.add_argument("-dt", "--samplingTime", required=False, 902 | help=" Data sampling time in seconds", default=0.004) 903 | 904 | args = vars(ap.parse_args()) 905 | 906 | 907 | main(args) 908 | --------------------------------------------------------------------------------