├── .github └── workflows │ └── lint_python.yml ├── autotune ├── arx_rls.py ├── autotune.py ├── closed_loop_test.py ├── data_extractor.py ├── data_selection_window.py ├── logs │ ├── quadrotor_sitl_jmavsim.ulg │ ├── quadrotor_x500.ulg │ └── vtol_standard.ulg ├── pid_design.py ├── pid_synthesis_symbolic.py ├── readme.md ├── simulated_autotune.py └── system_identification.py ├── control_allocation ├── README.md ├── mixer_convergence_vtol.py ├── mixer_multirotor.py └── mixerlib.py ├── drag_fusion_tuning ├── drag_fusion_symbolic.py ├── drag_replay.py ├── readme.md └── requirements.txt ├── filters └── digital_filter_compare.py ├── hover_thrust_estimator ├── HoverThrEstimator.py ├── hover_thrust_replay.py └── hover_thrust_sim.py ├── leaky_integrator ├── README.md └── leaky_integrator.py ├── quaternion_attitude_control ├── README.md └── quaternion_attitude_control_test.py ├── range_finder_kinematic_consistency ├── AlphaFilter.py ├── RangeFinderConsistencyCheck.py ├── data_extractor.py └── range_finder_consistency_check_replay.py ├── requirements.txt └── trajectory_generator ├── README.md ├── VelocitySmoothing.py ├── closed_loop_ziegler_nichols.py ├── images └── vel_traj_graph.png ├── velocity_trajectory_generator.py └── velocity_trajectory_generator_symbolic.py /.github/workflows/lint_python.yml: -------------------------------------------------------------------------------- 1 | name: lint_python 2 | on: [pull_request, push] 3 | jobs: 4 | lint_python: 5 | runs-on: ubuntu-latest 6 | steps: 7 | - uses: actions/checkout@v2 8 | - uses: actions/setup-python@v2 9 | with: 10 | python-version: '3.10.1' 11 | - run: pip install bandit black codespell flake8 isort mypy pytest pyupgrade safety 12 | - run: bandit --recursive --skip B101 . 13 | - run: black --check . || true 14 | - run: codespell # --ignore-words-list="" --skip="" 15 | - run: flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics 16 | - run: flake8 . --count --exit-zero --max-complexity=10 --max-line-length=88 --show-source --statistics 17 | - run: isort --check-only --profile black . || true 18 | - run: pip install -r requirements.txt 19 | - run: mypy --ignore-missing-imports --install-types --non-interactive . || true 20 | - run: pytest . || pytest --doctest-modules . || true 21 | - run: shopt -s globstar && pyupgrade --py36-plus **/*.py || true 22 | - run: safety check --full-report || true 23 | -------------------------------------------------------------------------------- /autotune/arx_rls.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Copyright (c) 2021 PX4 Development Team 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions 7 | are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in 13 | the documentation and/or other materials provided with the 14 | distribution. 15 | 3. Neither the name PX4 nor the names of its contributors may be 16 | used to endorse or promote products derived from this software 17 | without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | 32 | File: arx_rls.py 33 | Author: Mathieu Bresciani 34 | License: BSD 3-Clause 35 | Description: 36 | Efficient recursive weighted least-squares algorithm 37 | without matrix inversion 38 | 39 | Assumes an ARX (autoregressive) model: 40 | A(q^-1)y(k) = q^-d * B(q^-1)u(k) + A(q^-1)e(k) 41 | 42 | with: 43 | q^-i backward shift operator 44 | A(q^-1) = 1 + a_1*q^-1 +...+ a_n*q^-n 45 | B(q^-1) = b_0 + b_1*q^-1...+ b_m*q^-m 46 | n order of A(q^-1) 47 | m order of B(q^-1) 48 | d delay 49 | u input of the system 50 | y output of the system 51 | e white noise input 52 | 53 | usage: 54 | n = m = 2 55 | d = 1 56 | lbda = 0.95 # forgetting factor (0.9 < lbda < 0.99) 57 | rls = ArxRls(n, m, d, lbda) 58 | for i in range(0, n_steps): 59 | rls.update(u, y) 60 | theta_hat = rls._theta_hat 61 | 62 | theta_hat is of the form (a_1,..., a_n, b_0,..., b_n) 63 | 64 | References: 65 | - Identification de systemes dynamiques, D.Bonvin and A.Karimi, epfl, 2011 66 | """ 67 | 68 | import numpy as np 69 | 70 | class ArxRls(object): 71 | def __init__(self, n, m, d, lbda=1.0): 72 | self._n = n 73 | self._m = m 74 | self._d = d 75 | p = n+m+1 # number of parameters (+1 because of b_0) 76 | self._P = 10000 * np.asmatrix(np.eye(p)) # initialize to a large value 77 | self._theta_hat = np.transpose(np.asmatrix(np.zeros(n+m+1))) 78 | self._y = np.zeros(n+1) 79 | self._u = np.zeros(m+d+1) 80 | self._lbda = lbda 81 | 82 | def update(self, u, y): 83 | self.addInputOutput(u, y) 84 | phi = self.constructPhi() 85 | lbda = self._lbda 86 | self._P = (self._P - self._P * phi * phi.T * self._P / (lbda + phi.T * self._P * phi))/lbda # eq 3.66 87 | self._theta_hat += self._P * phi*(self._y[self._n] - phi.T * self._theta_hat) # eq 3.67 88 | 89 | return self._theta_hat, self._P 90 | 91 | def addInputOutput(self, u, y): 92 | self.shiftRegisters() 93 | self._y[self._n] = y 94 | self._u[self._m+self._d] = u 95 | 96 | def shiftRegisters(self): 97 | for i in range(0, self._n): 98 | self._y[i] = self._y[i+1] 99 | for i in range(0, self._m+self._d): 100 | self._u[i] = self._u[i+1] 101 | 102 | def constructPhi(self): 103 | phi_a = np.asmatrix([-self._y[self._n-(i+1)] for i in range(self._n)]) 104 | phi_b = np.asmatrix([self._u[self._m-i] for i in range(self._m+1)]) 105 | phi = (np.concatenate((phi_a, phi_b), axis=1)).T 106 | return phi 107 | 108 | if __name__ == '__main__': 109 | n = 2 110 | m = 1 111 | d = 1 112 | rls = ArxRls(n, m, d) 113 | 114 | assert len(rls._y) == n+1 115 | assert len(rls._u) == m+d+1 116 | rls.update(1, 2) 117 | rls.update(3, 4) 118 | rls.update(5, 6) 119 | assert rls._u.item(-1) == 5 120 | assert rls._y.item(-1) == 6 121 | phi = rls.constructPhi() 122 | assert len(phi) == 4 123 | assert phi.item(0) == -4 124 | assert phi.item(1) == -2 125 | assert phi.item(2) == 3 126 | assert phi.item(3) == 1 127 | print(rls._theta_hat) 128 | -------------------------------------------------------------------------------- /autotune/autotune.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Copyright (c) 2021-2024 PX4 Development Team 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions 7 | are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in 13 | the documentation and/or other materials provided with the 14 | distribution. 15 | 3. Neither the name PX4 nor the names of its contributors may be 16 | used to endorse or promote products derived from this software 17 | without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | 32 | File: autotune.py 33 | Author: Mathieu Bresciani 34 | License: BSD 3-Clause 35 | Description: 36 | UI tool for parametric system identification and controller design 37 | """ 38 | 39 | import sys 40 | from PyQt5.QtWidgets import QDialog, QApplication, QLabel, QRadioButton, QSlider, QPushButton, QVBoxLayout, QHBoxLayout, QFormLayout, QFileDialog, QLineEdit, QSpinBox, QDoubleSpinBox, QMessageBox, QCheckBox, QTableWidget, QTableWidgetItem, QHeaderView 41 | from PyQt5.QtCore import Qt 42 | 43 | from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas 44 | from matplotlib.backends.backend_qt5agg import NavigationToolbar2QT as NavigationToolbar 45 | import matplotlib.pyplot as plt 46 | import matplotlib.patches as mpatches 47 | 48 | from data_extractor import * 49 | from pid_design import computePidGmvc 50 | from system_identification import SystemIdentification 51 | import control as ctrl 52 | from scipy.signal import resample, detrend 53 | 54 | from data_selection_window import DataSelectionWindow 55 | 56 | class Window(QDialog): 57 | def __init__(self, parent=None): 58 | super(Window, self).__init__(parent) 59 | 60 | self.model_ref = None 61 | self.input_ref = None 62 | self.closed_loop_ref = None 63 | self.closed_loop_ax = None 64 | self.bode_plot_ref = None 65 | self.state_plot_refs= [] 66 | self.pz_plot_refs= [] 67 | self.file_name = None 68 | self.is_system_identified = False 69 | self.axis = 0 70 | self.dt = 0.005 71 | self.rise_time = 0.13 72 | self.damping_index = 0.0 73 | self.detune_coeff = 0.5 74 | self.kc = 0.0 75 | self.ki = 0.0 76 | self.kd = 0.0 77 | self.kff = 0.0 78 | self.figure = plt.figure(1) 79 | self.figure.subplots_adjust(hspace=0.5, wspace=1.0) 80 | self.num = [] 81 | self.den = [] 82 | self.sys_id_delays = 1 83 | self.sys_id_n_zeros = 2 84 | self.sys_id_n_poles = 2 85 | 86 | # this is the Canvas Widget that displays the `figure` 87 | # it takes the `figure` instance as a parameter to __init__ 88 | self.canvas = FigureCanvas(self.figure) 89 | 90 | # this is the Navigation widget 91 | # it takes the Canvas widget and a parent 92 | self.toolbar = NavigationToolbar(self.canvas, self) 93 | 94 | self.btn_open_log = QPushButton("Open log") 95 | self.btn_open_log.clicked.connect(self.loadLog) 96 | 97 | # set the layout 98 | layout_v = QVBoxLayout() 99 | layout_h = QHBoxLayout() 100 | left_menu = QVBoxLayout() 101 | left_menu.addWidget(self.btn_open_log) 102 | 103 | id_params_group = QFormLayout() 104 | self.line_edit_zeros = QSpinBox() 105 | self.line_edit_zeros.setValue(self.sys_id_n_zeros) 106 | self.line_edit_zeros.setRange(0, 6) 107 | self.line_edit_zeros.valueChanged.connect(self.onZerosChanged) 108 | id_params_group.addRow(QLabel("Zeros"), self.line_edit_zeros) 109 | self.line_edit_poles = QSpinBox() 110 | self.line_edit_poles.setValue(self.sys_id_n_poles) 111 | self.line_edit_poles.setRange(0, 6) 112 | self.line_edit_poles.valueChanged.connect(self.onPolesChanged) 113 | id_params_group.addRow(QLabel("Poles"), self.line_edit_poles) 114 | self.line_edit_delays = QSpinBox() 115 | self.line_edit_delays.setValue(self.sys_id_delays) 116 | self.line_edit_delays.setRange(0, 1000) 117 | self.line_edit_delays.valueChanged.connect(self.onDelaysChanged) 118 | id_params_group.addRow(QLabel("Delays"), self.line_edit_delays) 119 | self.line_edit_trim = QDoubleSpinBox() 120 | self.trim_airspeed = 20.0 121 | self.line_edit_trim.setValue(self.trim_airspeed) 122 | self.line_edit_trim.setRange(0.0, 100.0) 123 | self.line_edit_trim.textChanged.connect(self.onTrimChanged) 124 | self.line_edit_trim.setEnabled(False) 125 | id_params_group.addRow(QLabel("Trim airspeed"), self.line_edit_trim) 126 | self.btn_run_sys_id = QPushButton("Run identification") 127 | self.btn_run_sys_id.clicked.connect(self.onSysIdClicked) 128 | self.btn_run_sys_id.setEnabled(False) 129 | id_params_group.addRow(self.btn_run_sys_id) 130 | left_menu.addLayout(id_params_group) 131 | 132 | layout_tf = self.createTfLayout() 133 | left_menu.addLayout(layout_tf) 134 | 135 | offset_group = QFormLayout() 136 | self.line_edit_offset = QDoubleSpinBox() 137 | self.line_edit_offset.setValue(0.0) 138 | self.line_edit_offset.setRange(-10.0,10.0) 139 | self.line_edit_offset.textChanged.connect(self.onOffsetChanged) 140 | offset_group.addRow(QLabel("Offset"), self.line_edit_offset) 141 | left_menu.addLayout(offset_group) 142 | left_menu.addStretch(1) 143 | 144 | layout_gmvc = self.createGmvcLayout() 145 | layout_pid = self.createPidLayout() 146 | layout_controller = QHBoxLayout() 147 | layout_controller.addLayout(layout_gmvc) 148 | layout_controller.addLayout(layout_pid) 149 | 150 | layout_plot = QVBoxLayout() 151 | layout_h.addLayout(left_menu) 152 | layout_h.addLayout(layout_plot) 153 | layout_h.setStretch(1,1) 154 | layout_plot.addWidget(self.toolbar) 155 | layout_plot.addWidget(self.canvas) 156 | layout_v.addLayout(layout_h) 157 | layout_v.setStretch(0,1) 158 | layout_v.addLayout(layout_controller) 159 | self.setLayout(layout_v) 160 | 161 | def reset(self): 162 | self.model_ref = None 163 | self.input_ref = None 164 | self.closed_loop_ref = None 165 | self.bode_plot_ref = None 166 | self.state_plot_refs= [] 167 | self.pz_plot_refs= [] 168 | self.is_system_identified = False 169 | 170 | def createTfLayout(self): 171 | layout_tf = QVBoxLayout() 172 | self.t_coeffs = QTableWidget() 173 | self.t_coeffs.setColumnCount(1) 174 | self.t_coeffs.setHorizontalHeaderLabels(["Coefficients"]) 175 | self.t_coeffs.horizontalHeader().setSectionResizeMode(QHeaderView.Stretch) 176 | self.t_coeffs.verticalHeader().setSectionResizeMode(QHeaderView.Fixed) 177 | self.t_coeffs.setVerticalScrollBarPolicy(Qt.ScrollBarAlwaysOff) 178 | self.t_coeffs.setFixedWidth(120) 179 | self.t_coeffs.itemChanged.connect(self.onModelChanged) 180 | 181 | # Place in horizontal layout to center it properly 182 | self.updateCoeffTable() 183 | layout_coeff = QHBoxLayout() 184 | layout_coeff.addWidget(self.t_coeffs) 185 | layout_tf.addLayout(layout_coeff) 186 | 187 | layout_dt = QFormLayout() 188 | self.line_edit_dt = QLineEdit("0.0") 189 | self.line_edit_dt.textChanged.connect(self.onModelChanged) 190 | layout_dt.addRow(QLabel("dt"), self.line_edit_dt) 191 | layout_tf.addLayout(layout_dt) 192 | 193 | self.btn_update_model = QPushButton("Update model") 194 | self.btn_update_model.setEnabled(False) 195 | self.btn_update_model.clicked.connect(self.updateModel) 196 | layout_tf.addWidget(self.btn_update_model) 197 | return layout_tf 198 | 199 | def updateCoeffTable(self): 200 | self.t_coeffs.setRowCount(self.sys_id_n_poles + self.sys_id_n_zeros + 1) 201 | self.t_coeffs.clearContents() 202 | 203 | labels = [] 204 | 205 | for i in range(self.sys_id_n_poles): 206 | labels.append("a{}".format(i+1)) 207 | 208 | for i in range(self.sys_id_n_zeros+1): 209 | labels.append("b{}".format(i)) 210 | 211 | self.t_coeffs.setVerticalHeaderLabels(labels) 212 | self.t_coeffs.setFixedHeight(self.t_coeffs.verticalHeader().length() 213 | + self.t_coeffs.horizontalHeader().height() + 2) 214 | 215 | def onModelChanged(self): 216 | self.btn_update_model.setEnabled(True) 217 | 218 | def onTrimChanged(self): 219 | try: 220 | self.trim_airspeed = float(self.line_edit_trim.text()) 221 | except ValueError: 222 | self.trim_airspeed = 0 223 | self.line_edit_trim.setValue(self.trim_airspeed) 224 | 225 | self.btn_run_sys_id.setEnabled(True) 226 | self.plotInputOutput() 227 | 228 | def onOffsetChanged(self): 229 | self.plotInputOutput() 230 | 231 | def onDelaysChanged(self): 232 | self.btn_run_sys_id.setEnabled(True) 233 | 234 | def onPolesChanged(self): 235 | self.sys_id_n_poles = self.line_edit_poles.value() 236 | self.updateCoeffTable() 237 | self.btn_run_sys_id.setEnabled(True) 238 | 239 | def onZerosChanged(self): 240 | self.sys_id_n_zeros = self.line_edit_zeros.value() 241 | self.updateCoeffTable() 242 | self.btn_run_sys_id.setEnabled(True) 243 | 244 | def onSysIdClicked(self): 245 | n_poles = self.line_edit_poles.value() 246 | n_zeros = self.line_edit_zeros.value() 247 | self.sys_id_delays = self.line_edit_delays.value() 248 | 249 | if n_poles < n_zeros: 250 | n_poles = n_zeros 251 | self.printImproperTfError() 252 | 253 | else: 254 | self.sys_id_n_zeros = n_zeros 255 | self.sys_id_n_poles = n_poles 256 | self.runIdentification() 257 | self.computeController() 258 | 259 | def printImproperTfError(self): 260 | msg = QMessageBox() 261 | msg.setIcon(QMessageBox.Critical) 262 | msg.setWindowTitle("Error") 263 | msg.setText("Transfer function must be proper, set Poles >= Zeros") 264 | msg.exec_() 265 | 266 | def createPidLayout(self): 267 | layout_pid = QFormLayout() 268 | 269 | layout_structure = QHBoxLayout() 270 | layout_structure.addWidget(QLabel("Ideal/Standard: Kp * [1 + Ki + Kd]\t(Parallel: Kp + Ki + Kd)")) 271 | self.pid_no_zero_box = QCheckBox("PI no-zero", self) 272 | self.pid_no_zero_box.setChecked(False) 273 | self.pid_no_zero_box.stateChanged.connect(self.updateClosedLoop) 274 | layout_structure.addWidget(self.pid_no_zero_box) 275 | layout_pid.addRow(layout_structure) 276 | 277 | layout_k = QHBoxLayout() 278 | self.slider_k = DoubleSlider(Qt.Horizontal) 279 | self.slider_k.setMinimum(0.01) 280 | self.slider_k.setMaximum(4.0) 281 | self.slider_k.setInterval(0.01) 282 | self.lbl_k = QLabel("{:.2f} ({:.2f})".format(self.kc, self.kc)) 283 | layout_k.addWidget(self.slider_k) 284 | layout_k.addWidget(self.lbl_k) 285 | self.slider_k.valueChanged.connect(self.updateLabelK) 286 | layout_pid.addRow(QLabel("K"), layout_k) 287 | 288 | layout_i = QHBoxLayout() 289 | self.slider_i = DoubleSlider(Qt.Horizontal) 290 | self.slider_i.setMinimum(0.0) 291 | self.slider_i.setMaximum(20.0) 292 | self.slider_i.setInterval(0.1) 293 | self.lbl_i = QLabel("{:.2f} ({:.2f})".format(self.ki, self.kc * self.ki)) 294 | layout_i.addWidget(self.slider_i) 295 | layout_i.addWidget(self.lbl_i) 296 | self.slider_i.valueChanged.connect(self.updateLabelI) 297 | layout_pid.addRow(QLabel("I"), layout_i) 298 | 299 | layout_d = QHBoxLayout() 300 | self.slider_d = DoubleSlider(Qt.Horizontal) 301 | self.slider_d.setMinimum(0.0) 302 | self.slider_d.setMaximum(0.2) 303 | self.slider_d.setInterval(0.001) 304 | self.lbl_d = QLabel("{:.3f} ({:.4f})".format(self.kd, self.kc * self.kd)) 305 | layout_d.addWidget(self.slider_d) 306 | layout_d.addWidget(self.lbl_d) 307 | self.slider_d.valueChanged.connect(self.updateLabelD) 308 | layout_pid.addRow(QLabel("D"), layout_d) 309 | 310 | layout_ff = QHBoxLayout() 311 | self.slider_ff = DoubleSlider(Qt.Horizontal) 312 | self.slider_ff.setMinimum(0.0) 313 | self.slider_ff.setMaximum(5.0) 314 | self.slider_ff.setInterval(0.01) 315 | self.lbl_ff = QLabel("{:.3f} ({:.3f})".format(self.kff, self.kff)) 316 | layout_ff.addWidget(self.slider_ff) 317 | layout_ff.addWidget(self.lbl_ff) 318 | self.slider_ff.valueChanged.connect(self.updateLabelFF) 319 | layout_pid.addRow(QLabel("FF"), layout_ff) 320 | 321 | return layout_pid 322 | 323 | def updateLabelK(self): 324 | self.kc = self.slider_k.value() 325 | self.lbl_k.setText("{:.2f} ({:.2f})".format(self.kc, self.kc)) 326 | 327 | # Kc also modifies the Ki and Kd gains of the parallel form 328 | self.lbl_i.setText("{:.2f} ({:.2f})".format(self.ki, self.kc * self.ki)) 329 | self.lbl_d.setText("{:.3f} ({:.4f})".format(self.kd, self.kc * self.kd)) 330 | if self.slider_k.isSliderDown(): 331 | self.updateClosedLoop() 332 | 333 | def updateLabelI(self): 334 | self.ki = self.slider_i.value() 335 | self.lbl_i.setText("{:.2f} ({:.2f})".format(self.ki, self.kc * self.ki)) 336 | if self.slider_i.isSliderDown(): 337 | self.updateClosedLoop() 338 | 339 | def updateLabelD(self): 340 | self.kd = self.slider_d.value() 341 | self.lbl_d.setText("{:.3f} ({:.4f})".format(self.kd, self.kc * self.kd)) 342 | if self.slider_d.isSliderDown(): 343 | self.updateClosedLoop() 344 | 345 | def updateLabelFF(self): 346 | self.kff = self.slider_ff.value() 347 | self.lbl_ff.setText("{:.3f} ({:.3f})".format(self.kff, self.kff)) 348 | if self.slider_ff.isSliderDown(): 349 | self.updateClosedLoop() 350 | 351 | def createGmvcLayout(self): 352 | layout_gmvc = QFormLayout() 353 | 354 | layout_rise_time = QHBoxLayout() 355 | self.slider_rise_time = DoubleSlider(Qt.Horizontal) 356 | self.slider_rise_time.setMinimum(0.01) 357 | self.slider_rise_time.setMaximum(1.0) 358 | self.slider_rise_time.setInterval(0.01) 359 | self.slider_rise_time.setValue(self.rise_time) 360 | self.lbl_rise_time = QLabel("{:.2f}".format(self.rise_time)) 361 | layout_rise_time.addWidget(self.slider_rise_time) 362 | layout_rise_time.addWidget(self.lbl_rise_time) 363 | self.slider_rise_time.valueChanged.connect(self.updateLabelRiseTime) 364 | layout_gmvc.addRow(QLabel("Rise time"), layout_rise_time) 365 | 366 | layout_damping = QHBoxLayout() 367 | self.slider_damping = DoubleSlider(Qt.Horizontal) 368 | self.slider_damping.setMinimum(0.0) 369 | self.slider_damping.setMaximum(2.0) 370 | self.slider_damping.setInterval(0.1) 371 | self.slider_damping.setValue(self.damping_index) 372 | self.lbl_damping = QLabel("{:.1f}".format(self.damping_index)) 373 | layout_damping.addWidget(self.slider_damping) 374 | layout_damping.addWidget(self.lbl_damping) 375 | self.slider_damping.valueChanged.connect(self.updateLabelDamping) 376 | layout_gmvc.addRow(QLabel("Damping index"), layout_damping) 377 | 378 | layout_detune = QHBoxLayout() 379 | self.slider_detune = DoubleSlider(Qt.Horizontal) 380 | self.slider_detune.setMinimum(0.0) 381 | self.slider_detune.setMaximum(2.0) 382 | self.slider_detune.setInterval(0.1) 383 | self.slider_detune.setValue(self.detune_coeff) 384 | self.lbl_detune = QLabel("{:.1f}".format(self.detune_coeff)) 385 | layout_detune.addWidget(self.slider_detune) 386 | layout_detune.addWidget(self.lbl_detune) 387 | self.slider_detune.valueChanged.connect(self.updateLabelDetune) 388 | layout_gmvc.addRow(QLabel("Detune coeff"), layout_detune) 389 | return layout_gmvc 390 | 391 | def updateLabelRiseTime(self): 392 | self.rise_time = self.slider_rise_time.value() 393 | self.lbl_rise_time.setText("{:.2f}".format(self.rise_time)) 394 | if self.slider_rise_time.isSliderDown(): 395 | self.computeController() 396 | 397 | def updateLabelDamping(self): 398 | self.damping_index = self.slider_damping.value() 399 | self.lbl_damping.setText("{:.1f}".format(self.damping_index)) 400 | if self.slider_damping.isSliderDown(): 401 | self.computeController() 402 | 403 | def updateLabelDetune(self): 404 | self.detune_coeff = self.slider_detune.value() 405 | self.lbl_detune.setText("{:.1f}".format(self.detune_coeff)) 406 | if self.slider_detune.isSliderDown(): 407 | self.computeController() 408 | 409 | def runIdentification(self): 410 | n_steps = len(self.t) 411 | 412 | n = self.sys_id_n_poles # order of the denominator (a_1,...,a_n) 413 | m = self.sys_id_n_zeros # order of the numerator (b_0,...,b_m) 414 | d = self.sys_id_delays # number of delays 415 | tau = 60.0 # forgetting period 416 | lbda = 1.0 - self.dt/tau 417 | self.sysid = SystemIdentification(n, m, d) 418 | self.sysid.lbda = lbda 419 | 420 | (theta_hat, a_coeffs, b_coeffs) = self.sysid.run(self.t, self.u, self.y) 421 | 422 | self.plotStateVector(a_coeffs, b_coeffs) 423 | self.is_system_identified = True 424 | self.btn_run_sys_id.setEnabled(False) 425 | dt = self.dt 426 | # num = self.sysid.getNum() 427 | # den = self.sysid.getDen() 428 | # self.Gz_dot = ctrl.TransferFunction(num, den, dt) 429 | 430 | # Uncomment below to add integrator 431 | # self.sysid.addIntegrator() 432 | self.num = self.sysid.getNum() 433 | 434 | self.den = self.sysid.getDen() 435 | 436 | self.Gz = ctrl.TransferFunction(self.num, self.den, dt) 437 | self.updateTfDisplay(a_coeffs[:, -1], b_coeffs[:, -1]) 438 | self.plotPolesZeros() 439 | self.replayInputData() 440 | 441 | def plotStateVector(self, a_coeffs, b_coeffs): 442 | t = self.t 443 | ax = self.figure.add_subplot(3,3,(4,5)) 444 | legend = [] 445 | 446 | for i in range(len(a_coeffs)): 447 | ax.plot(t, a_coeffs[i]) 448 | legend.append("a{}".format(i+1)) 449 | 450 | for i in range(len(b_coeffs)): 451 | ax.plot(t, b_coeffs[i]) 452 | legend.append("b{}".format(i)) 453 | 454 | ax.set_title("Parameter identification") 455 | ax.set_xlabel("Time (s)") 456 | ax.legend(legend, loc='lower left') 457 | 458 | self.canvas.draw() 459 | 460 | def replayInputData(self): 461 | if not self.is_system_identified: 462 | return 463 | d = self.sysid.d 464 | u_detrended = detrend(self.u) 465 | u_delayed = np.concatenate(([0 for k in range(d)], u_detrended[0:(len(u_detrended)-d)])) 466 | self.t_est, self.y_est = ctrl.forced_response(self.Gz, T=self.t, U=u_delayed) 467 | if len(self.t_est) > len(self.y_est): 468 | self.t_est = self.t_est[0:len(self.y_est - 1)] 469 | self.plotInputOutput() 470 | 471 | def updateTfDisplay(self, a_coeffs, b_coeffs): 472 | 473 | for i in range(self.sys_id_n_poles): 474 | self.t_coeffs.setItem(i, 0, QTableWidgetItem("{:.6f}".format(a_coeffs[i]))) 475 | 476 | for i in range(self.sys_id_n_zeros + 1): 477 | self.t_coeffs.setItem(self.sys_id_n_poles + i, 0, QTableWidgetItem("{:.6f}".format(b_coeffs[i]))) 478 | 479 | dt = self.Gz.dt 480 | self.line_edit_dt.setText("{:.4f}".format(dt)) 481 | self.btn_update_model.setEnabled(False) 482 | 483 | def plotPolesZeros(self): 484 | if not self.is_system_identified: 485 | return 486 | poles = self.Gz.poles() 487 | zeros = self.Gz.zeros() 488 | if not self.pz_plot_refs: 489 | ax = self.figure.add_subplot(3,3,6) 490 | plot_ref = ax.plot(poles.real, poles.imag, 'rx', markersize=10) 491 | self.pz_plot_refs.append(plot_ref[0]) 492 | plot_ref = ax.plot(zeros.real, zeros.imag, 'ro', markersize=10) 493 | self.pz_plot_refs.append(plot_ref[0]) 494 | uc = mpatches.Circle((0,0), radius=1, fill=False, 495 | color='black', ls='dashed') 496 | ax.add_patch(uc) 497 | ax.axhline(0, color ="black", linestyle ="--") 498 | ax.axvline(0, color ="black", linestyle ="--") 499 | ax.set_xlim(-1.5, 1.5) 500 | ax.set_ylim(-1.5, 1.5) 501 | ax.set_aspect(1.0) 502 | ax.set_xlabel("Real") 503 | ax.set_ylabel("Imag") 504 | ax.set_title("Pole-Zero Map") 505 | else: 506 | self.pz_plot_refs[0].set_xdata(poles.real) 507 | self.pz_plot_refs[0].set_ydata(poles.imag) 508 | self.pz_plot_refs[1].set_xdata(zeros.real) 509 | self.pz_plot_refs[1].set_ydata(zeros.imag) 510 | 511 | self.canvas.draw() 512 | 513 | def updateModel(self): 514 | self.btn_run_sys_id.setEnabled(True) 515 | 516 | self.den = [1.0] 517 | self.num = [] 518 | 519 | for i in range(self.sys_id_n_poles): 520 | val = float(self.t_coeffs.item(i, 0).text()) 521 | self.den.append(val) 522 | 523 | for i in range(self.sys_id_n_zeros+1): 524 | val = float(self.t_coeffs.item(self.sys_id_n_poles + i, 0).text()) 525 | self.num.append(val) 526 | 527 | dt = float(self.line_edit_dt.text()) 528 | self.dt = dt 529 | self.Gz = ctrl.TransferFunction(self.num, self.den, self.dt) 530 | self.resampleData(dt) 531 | self.is_system_identified = True 532 | self.plotPolesZeros() 533 | self.replayInputData() 534 | self.computeController() 535 | self.btn_update_model.setEnabled(False) 536 | return 537 | 538 | def computeController(self): 539 | if not self.is_system_identified: 540 | return 541 | sigma = self.rise_time # rise time 542 | delta = self.damping_index # damping property, set between 0 and 2 (1 for Butterworth) 543 | lbda = self.detune_coeff 544 | (self.kc, self.ki, self.kd) = computePidGmvc(self.num, self.den, self.dt, sigma, delta, lbda) 545 | #TODO:find a better solution 546 | self.ki /= 5.0 547 | static_gain = sum(self.num) / sum(self.den) 548 | self.kff = 1 / static_gain 549 | 550 | self.updateKIDSliders() 551 | self.updateClosedLoop() 552 | 553 | def updateKIDSliders(self): 554 | self.slider_k.setValue(self.kc) 555 | self.slider_i.setValue(self.ki) 556 | self.slider_d.setValue(self.kd) 557 | self.slider_ff.setValue(self.kff) 558 | 559 | def updateClosedLoop(self): 560 | if not self.is_system_identified: 561 | return 562 | # Simulate closed-loop system with generated PID 563 | num = self.num 564 | den = self.den 565 | dt = self.dt 566 | kc = self.kc 567 | ki = self.ki 568 | kd = self.kd 569 | kff = self.kff 570 | 571 | delays = ctrl.TransferFunction([1], np.append([1], np.zeros(self.sys_id_delays)), dt, inputs='r', outputs='rd') 572 | plant = ctrl.TransferFunction(num, den, dt, inputs='u', outputs='plant_out') 573 | sampler = ctrl.TransferFunction([1], [1, 0], dt, inputs='plant_out', outputs='y') 574 | sum_feedback = ctrl.summing_junction(inputs=['rd', '-y'], output='e') 575 | 576 | # Default is standard PID 577 | feedforward = ctrl.TransferFunction([kff], [1], dt, inputs='rd', outputs='ff_out') 578 | 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)) 579 | 580 | # Derivative with 1st order LPF (discretized using Euler method: s = (z-1)/dt) 581 | derivative_cutoff_freq = 10.0 # Hz 582 | tau = 1 / (2 * np.pi * derivative_cutoff_freq) 583 | derivative_num = np.array([kd , -kd]) 584 | derivative_den = np.array([tau, -tau + dt]) 585 | d_control = ctrl.TransferFunction(derivative_num, derivative_den, dt, inputs='e', outputs='d_out') 586 | 587 | id_control = ctrl.summing_junction(inputs=['e', 'i_out', 'd_out'], output='id_out') 588 | p_control = ctrl.TransferFunction([kc], [1], dt, inputs='id_out', outputs='pid_out') 589 | sum_control = ctrl.summing_junction(inputs=['pid_out', 'ff_out'], output='u') 590 | 591 | remove_zero = self.pid_no_zero_box.isChecked() 592 | no_derivative_kick = True 593 | 594 | if remove_zero: 595 | # P on feedback only to remove zero (3-loop autopilot style) 596 | id_control = ctrl.summing_junction(inputs=['-y', 'i_out', 'd_out'], output='id_out') 597 | 598 | if no_derivative_kick: 599 | # Derivative on feedback only to remove the "derivative kick" 600 | d_control = ctrl.TransferFunction(-derivative_num, derivative_den, dt, inputs='y', outputs='d_out') 601 | 602 | closed_loop = ctrl.interconnect([delays, sampler, sum_feedback, feedforward, sum_control, p_control, i_control, d_control, id_control, plant], inputs='r', outputs='y') 603 | 604 | t_out,y_out = ctrl.step_response(closed_loop, T=np.arange(0,2,dt)) 605 | 606 | # Add disturbance 607 | sum_feedback_no_ref = ctrl.summing_junction(inputs=['-y'], output='e') 608 | sum_control_with_disturbance = ctrl.summing_junction(inputs=['pid_out', 'disturbance'], output='u') 609 | disturbance_loop = ctrl.interconnect([sampler, sum_feedback_no_ref, sum_control_with_disturbance, p_control, i_control, d_control, id_control, plant], inputs='disturbance', outputs='y') 610 | d = np.zeros_like(t_out) 611 | d[t_out >= 1.0] = -0.05 #TODO: parameterize 612 | _, y_d = ctrl.forced_response(disturbance_loop, t_out, d) 613 | y_out += y_d 614 | 615 | self.plotClosedLoop(t_out, y_out) 616 | w = np.logspace(-1, 3, 40).tolist() 617 | mag, phase, omega = ctrl.bode(plant, omega=np.asarray(w), plot=False) 618 | mag_cl, phase_cl, omega_cl = ctrl.bode(closed_loop, omega=np.asarray(w), plot=False) 619 | self.plotBode(omega, mag, omega_cl, mag_cl) 620 | 621 | def plotClosedLoop(self, t, y): 622 | if self.closed_loop_ref is None: 623 | ax = self.figure.add_subplot(3,3,7) 624 | ax.step(t, [1 if i>0 else 0 for i in t], 'k--') 625 | plot_ref = ax.plot(t, y) 626 | self.closed_loop_ref = plot_ref[0] 627 | self.closed_loop_ax = ax 628 | ax.set_title("Closed-loop step response") 629 | ax.set_xlabel("Time (s)") 630 | ax.set_ylabel("Amplitude (rad/s)") 631 | else: 632 | self.closed_loop_ref.set_xdata(t) 633 | self.closed_loop_ref.set_ydata(y) 634 | self.closed_loop_ax.set_ylim(np.min(y),np.max([1.5, np.max(y)])) 635 | 636 | self.canvas.draw() 637 | 638 | def plotBode(self, w, mag, w_cl, mag_cl): 639 | if self.bode_plot_ref is None: 640 | ax = self.figure.add_subplot(3,3,(8,9)) 641 | f = w_cl/(2*np.pi) 642 | plot_ref = ax.semilogx(f, 10 * np.log10(mag_cl)) 643 | ax.plot([f[0], f[-1]], [0, 0], 'k--') 644 | ax.plot([f[0], f[-1]], [-3, -3], 'g--') 645 | self.bode_plot_ref = plot_ref[0] 646 | ax.set_title("Bode") 647 | ax.set_xlabel("Frequency (Hz)") 648 | ax.set_ylabel("Magnitude (dB)") 649 | else: 650 | f = w_cl/(2*np.pi) 651 | self.bode_plot_ref.set_xdata(f) 652 | self.bode_plot_ref.set_ydata(10 * np.log10(mag_cl)) 653 | 654 | self.canvas.draw() 655 | 656 | def plotInputOutput(self, redraw=False): 657 | if len(self.true_airspeed) == len(self.input): 658 | scale = np.array(self.true_airspeed) / self.trim_airspeed 659 | self.u = self.input * scale**2 660 | self.line_edit_trim.setEnabled(True) 661 | 662 | if self.model_ref is None or redraw: 663 | # First time we have no plot reference, so do a normal plot. 664 | # .plot returns a list of line s, as we're 665 | # only getting one we can take the first element. 666 | self.figure.clear() 667 | ax = self.figure.add_subplot(3,3,(1,3)) 668 | input_ref = ax.plot(self.t, self.u) 669 | self.input_ref = input_ref[0] 670 | ax.plot(self.t, self.y) 671 | plot_refs = ax.plot(0, 0) 672 | self.model_ref = plot_refs[0] 673 | ax.set_title("Logged data") 674 | ax.set_xlabel("Time (s)") 675 | ax.set_ylabel("Amplitude") 676 | ax.legend(["Input", "Output", "Model"]) 677 | else: 678 | # We have a reference, we can use it to update the data for that line. 679 | self.model_ref.set_xdata(self.t_est) 680 | try: 681 | offset = float(self.line_edit_offset.text()) 682 | except ValueError: 683 | offset = 0 684 | 685 | self.model_ref.set_ydata(self.y_est + offset) 686 | self.input_ref.set_ydata(self.u) 687 | 688 | self.canvas.draw() 689 | 690 | def loadLog(self): 691 | options = QFileDialog.Options() 692 | options |= QFileDialog.DontUseNativeDialog 693 | self.file_name, _ = QFileDialog.getOpenFileName(self,"QFileDialog.getOpenFileName()", "","ULog (*.ulg)", options=options) 694 | 695 | if self.file_name: 696 | select = DataSelectionWindow(self.file_name) 697 | 698 | if select.exec_(): 699 | self.reset() 700 | self.t = select.t - select.t[0] 701 | self.input = select.u 702 | self.u = self.input 703 | self.y = select.y 704 | self.true_airspeed = select.v 705 | self.refreshInputOutputData() 706 | self.runIdentification() 707 | self.computeController() 708 | 709 | def refreshInputOutputData(self): 710 | self.reset() 711 | if self.file_name: 712 | dt = max(get_delta_mean(self.t), 0.008) 713 | self.resampleData(dt) 714 | self.plotInputOutput(redraw=True) 715 | 716 | def resampleData(self, dt): 717 | self.dt = dt 718 | self.t = np.arange(0, self.t[-1]+self.dt, self.dt) 719 | self.u = resample(self.u, len(self.t)) 720 | self.y = resample(self.y, len(self.t)) 721 | self.input = resample(self.input, len(self.t)) 722 | 723 | if len(self.true_airspeed) > 0: 724 | self.true_airspeed = resample(self.true_airspeed, len(self.t)) 725 | 726 | class DoubleSlider(QSlider): 727 | 728 | def __init__(self, *args, **kargs): 729 | super(DoubleSlider, self).__init__( *args, **kargs) 730 | self._min = 0 731 | self._max = 99 732 | self.interval = 1 733 | 734 | def setValue(self, value): 735 | index = round((value - self._min) / self.interval) 736 | return super(DoubleSlider, self).setValue(int(index)) 737 | 738 | def value(self): 739 | return self.index * self.interval + self._min 740 | 741 | @property 742 | def index(self): 743 | return super(DoubleSlider, self).value() 744 | 745 | def setIndex(self, index): 746 | return super(DoubleSlider, self).setValue(index) 747 | 748 | def setMinimum(self, value): 749 | self._min = value 750 | self._range_adjusted() 751 | 752 | def setMaximum(self, value): 753 | self._max = value 754 | self._range_adjusted() 755 | 756 | def setInterval(self, value): 757 | # To avoid division by zero 758 | if not value: 759 | raise ValueError('Interval of zero specified') 760 | self.interval = value 761 | self._range_adjusted() 762 | 763 | def _range_adjusted(self): 764 | number_of_steps = int((self._max - self._min) / self.interval) 765 | super(DoubleSlider, self).setMaximum(number_of_steps) 766 | 767 | 768 | if __name__ == '__main__': 769 | app = QApplication(sys.argv) 770 | 771 | main = Window() 772 | main.show() 773 | 774 | sys.exit(app.exec_()) 775 | -------------------------------------------------------------------------------- /autotune/closed_loop_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Copyright (c) 2021 PX4 Development Team 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions 7 | are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in 13 | the documentation and/or other materials provided with the 14 | distribution. 15 | 3. Neither the name PX4 nor the names of its contributors may be 16 | used to endorse or promote products derived from this software 17 | without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | 32 | File: autotune_sim.py 33 | Author: Mathieu Bresciani 34 | License: BSD 3-Clause 35 | Description: 36 | Simulate closed-loop system with generated PID controller 37 | """ 38 | import numpy as np 39 | import control as ctrl 40 | import matplotlib.pylab as plt 41 | from pid_design import computePidGmvc, gainsToNumDen 42 | 43 | # Discrete-time model given by system identification 44 | ## jMavsim 45 | # num = [0.167, -0.578, 0.651] 46 | # den = [ 1. , -1.689, 0.689] 47 | # Gazebo standard vtol 48 | # num = [0.086, -0.255, 0.231] 49 | # den = [ 1. , -1.864, 0.864] 50 | # dt = 0.0036 51 | 52 | # # 2nd order model 53 | # dt = 0.0036 54 | # zeta = 0.5 55 | # f_n = 1.0 56 | # w_n = 2*np.pi*f_n 57 | # pa1 = -zeta*w_n+w_n*np.sqrt((1-zeta**2))*1j 58 | # pa2 = -zeta*w_n-w_n*np.sqrt((1-zeta**2))*1j 59 | # p1 = np.exp(pa1*dt) 60 | # p2 = np.exp(pa2*dt) 61 | # num = [1] 62 | # den = [1, -(p1+p2), p1*p2] 63 | # K = sum(den)/sum(num) 64 | # num = [K] 65 | 66 | # 1st order model + integrator 67 | dt = 0.001 68 | f_n = 2.0 69 | w_n = 2*np.pi*f_n 70 | pz1 = np.exp(-w_n*dt) 71 | pz2 = 1 # integrator 72 | den = np.convolve([1, -pz1], [1, -pz2]) 73 | num = [100*(1-pz1)*dt] 74 | 75 | sigma = 0.05 # rise time 76 | delta = 0.5 # damping property, set between 0 and 2 (1 for Butterworth) 77 | lbda = 0 78 | (kc, ki, kd) = computePidGmvc(num, den, dt, sigma, delta, lbda) 79 | print("Standard: kc = {}, ki = {}, kd = {}\n".format(kc, ki, kd)) 80 | print("Parallel: kp = {}, ki = {}, kd = {}\n".format(kc, kc*ki, kc*kd)) 81 | 82 | # Compute reference model (just for plotting) 83 | rho = dt/sigma 84 | mu = 0.25 * (1-delta) + 0.51 * delta 85 | p1 = -2*np.exp(-rho/(2*mu))*np.cos((np.sqrt(4*mu-1)*rho/(2*mu))) 86 | p2 = np.exp(-rho/mu) 87 | P = ctrl.TransferFunction([1+p1+p2],[1, p1, p2], dt) 88 | 89 | Gz2 = ctrl.TransferFunction(num, den, dt) 90 | (pid_num, pid_den) = gainsToNumDen(kc, ki, kd, dt) 91 | PID = ctrl.TransferFunction(pid_num, pid_den, dt) 92 | Gcl = ctrl.feedback(PID * Gz2, 1) 93 | 94 | # Simulate step response 95 | t_ref, y_ref = ctrl.step_response(P, T=np.arange(0,1,dt)) 96 | t_out, y_out = ctrl.step_response(Gcl, T=np.arange(0,1,dt)) 97 | 98 | plt.plot(t_out, y_out) 99 | plt.plot(t_ref, y_ref) 100 | plt.title("Closed-loop step response") 101 | plt.xlabel("Time (s)") 102 | plt.ylabel("Amplitude (rad/s)") 103 | plt.legend(["Closed-loop", "Reference model"]) 104 | plt.show() 105 | -------------------------------------------------------------------------------- /autotune/data_extractor.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Copyright (c) 2021 PX4 Development Team 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions 7 | are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in 13 | the documentation and/or other materials provided with the 14 | distribution. 15 | 3. Neither the name PX4 nor the names of its contributors may be 16 | used to endorse or promote products derived from this software 17 | without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | 32 | File: data_extractor.py 33 | Author: Mathieu Bresciani 34 | License: BSD 3-Clause 35 | Description: 36 | rate controller auto-tuning algorithm test on real data 37 | """ 38 | 39 | import numpy as np 40 | from scipy import signal 41 | from pyulog import ULog 42 | from scipy.signal import resample 43 | 44 | def getInputOutputData(logfile, axis, t_start=0.0, t_stop=0.0, instance=0): 45 | log = ULog(logfile) 46 | 47 | y_data = get_data(log, 'vehicle_angular_velocity', 'xyz[{}]'.format(axis)) 48 | t_y_data = us2s(get_data(log, 'vehicle_angular_velocity', 'timestamp')) 49 | 50 | u_data = get_data(log, 'vehicle_torque_setpoint', 'xyz[{}]'.format(axis)) 51 | t_u_data = us2s(get_data(log, 'vehicle_torque_setpoint', 'timestamp')) 52 | 53 | v_data = get_data(log, 'airspeed_validated', 'true_airspeed_m_s') 54 | t_v_data = us2s(get_data(log, 'airspeed_validated', 'timestamp')) 55 | 56 | if not np.any(u_data): 57 | # Check for legacy topics 58 | actuator_controls_n = 'actuator_controls_{}'.format(instance) 59 | u_data = get_data(log, actuator_controls_n, 'control[{}]'.format(axis)) 60 | t_u_data = us2s(get_data(log, actuator_controls_n, 'timestamp')) 61 | 62 | (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) 63 | 64 | return (t_aligned, u_aligned, y_aligned, v_aligned) 65 | 66 | def get_data(log, topic_name, variable_name, instance=0): 67 | variable_data = np.array([]) 68 | for elem in log.data_list: 69 | if elem.name == topic_name: 70 | if instance == elem.multi_id: 71 | variable_data = elem.data[variable_name] 72 | break 73 | 74 | return variable_data 75 | 76 | def us2s(time_ms): 77 | return time_ms * 1e-6 78 | 79 | def get_delta_mean(data_list): 80 | dx = 0 81 | length = len(data_list) 82 | for i in range(1,length): 83 | dx = dx + (data_list[i]-data_list[i-1]) 84 | 85 | dx = dx/(length-1) 86 | return dx 87 | 88 | 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): 89 | status_data = get_data(log, 'autotune_attitude_control_status', 'state') 90 | t_status = us2s(get_data(log, 'autotune_attitude_control_status', 'timestamp')) 91 | 92 | len_y = len(t_y_data) 93 | len_s = len(t_status) 94 | len_v = len(t_v_data) 95 | i_y = 0 96 | i_s = 0 97 | i_v = 0 98 | u_aligned = [] 99 | y_aligned = [] 100 | v_aligned = [] 101 | t_aligned = [] 102 | axis_to_state = [2, 4, 6] # roll, pitch, yaw states 103 | 104 | if t_start == 0.0: 105 | t_start = t_u_data[0] 106 | 107 | if t_stop == 0.0: 108 | t_stop = t_u_data[-1] 109 | 110 | for i_u in range(len(t_u_data)): 111 | t_u = t_u_data[i_u] 112 | while t_y_data[i_y] <= t_u and i_y < len_y-1: 113 | i_y += 1 114 | 115 | while i_v < len_v-1 and t_v_data[i_v] <= t_u: 116 | i_v += 1 117 | 118 | if len_s > 0: 119 | while t_status[i_s] <= t_u and i_s < len_s-1: 120 | i_s += 1 121 | 122 | status_aligned = status_data[i_s-1] 123 | 124 | if status_aligned == axis_to_state[axis] and t_u >= t_start and t_u <= t_stop: 125 | u_aligned.append(u_data[i_u]) 126 | y_aligned.append(y_data[i_y-1]) 127 | t_aligned.append(t_u) 128 | 129 | if i_v > 0: 130 | v_aligned.append(v_data[i_v-1]) 131 | 132 | elif t_u >= t_start and t_u <= t_stop: 133 | u_aligned.append(u_data[i_u]) 134 | y_aligned.append(y_data[i_y-1]) 135 | t_aligned.append(t_u) 136 | 137 | if i_v > 0: 138 | v_aligned.append(v_data[i_v-1]) 139 | 140 | return (t_aligned, u_aligned, y_aligned, v_aligned) 141 | 142 | def printCppArrays(t_aligned, u_aligned, y_aligned): 143 | # Print data in c++ arrays 144 | # TODO: print to file and trigger from GUI using an "export" button 145 | n_samples = len(t_aligned) 146 | u_array = 'static constexpr float u_data[{}] = {{'.format(n_samples) 147 | y_array = 'static constexpr float y_data[{}] = {{'.format(n_samples) 148 | t_array = 'static constexpr float t_data[{}] = {{'.format(n_samples) 149 | 150 | for u in u_aligned: 151 | u_array += '{}f, '.format(u) 152 | 153 | for y in y_aligned: 154 | y_array += '{}f, '.format(y) 155 | 156 | for t in t_aligned: 157 | t_array += '{}f, '.format(t) 158 | 159 | u_array += '};' 160 | y_array += '};' 161 | t_array += '};' 162 | 163 | print('\n') 164 | print(u_array) 165 | print('\n') 166 | print(y_array) 167 | print('\n') 168 | print(t_array) 169 | 170 | if __name__ == '__main__': 171 | import argparse 172 | import os 173 | 174 | parser = argparse.ArgumentParser( 175 | description='Extract identification data from a give .ulg file') 176 | 177 | parser.add_argument('logfile', help='Full ulog file path, name and extension', type=str) 178 | parser.add_argument('--axis', dest='axis', choices=['x', 'y', 'z'], help='the body axis on interest') 179 | args = parser.parse_args() 180 | 181 | logfile = os.path.abspath(args.logfile) # Convert to absolute path 182 | axis = {'x':0, 'y':1, 'z':2}[args.axis] 183 | 184 | (t_aligned, u_aligned, y_aligned, v_aligned) = getInputOutputData(logfile, axis, instance=0) 185 | printCppArrays(t_aligned, u_aligned, y_aligned) 186 | -------------------------------------------------------------------------------- /autotune/data_selection_window.py: -------------------------------------------------------------------------------- 1 | from PyQt5.QtWidgets import QDialog, QVBoxLayout, QHBoxLayout, QPushButton, QLabel, QFormLayout, QRadioButton, QMessageBox 2 | 3 | import matplotlib.pyplot as plt 4 | from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas 5 | from matplotlib.widgets import SpanSelector 6 | 7 | import numpy as np 8 | 9 | from data_extractor import getInputOutputData 10 | 11 | class DataSelectionWindow(QDialog): 12 | def __init__(self, filename): 13 | QDialog.__init__(self) 14 | 15 | self.file_name = filename 16 | 17 | self.figure = plt.figure(1) 18 | self.canvas = FigureCanvas(self.figure) 19 | 20 | layout_v = QVBoxLayout() 21 | 22 | layout_v.addWidget(self.canvas) 23 | 24 | xyz_group = QHBoxLayout() 25 | r_x = QRadioButton("x") 26 | r_x.setChecked(True) 27 | r_y = QRadioButton("y") 28 | r_z = QRadioButton("z") 29 | xyz_group.addWidget(QLabel("Axis")) 30 | xyz_group.addWidget(r_x) 31 | xyz_group.addWidget(r_y) 32 | xyz_group.addWidget(r_z) 33 | r_x.clicked.connect(self.loadXData) 34 | r_y.clicked.connect(self.loadYData) 35 | r_z.clicked.connect(self.loadZData) 36 | 37 | layout_v.addLayout(xyz_group) 38 | 39 | btn_ok = QPushButton("Load selection") 40 | btn_ok.clicked.connect(self.loadLog) 41 | layout_v.addWidget(btn_ok) 42 | 43 | self.setLayout(layout_v) 44 | 45 | self.refreshInputOutputData() 46 | 47 | def loadLog(self): 48 | if self.t_stop > self.t_start: 49 | (self.t, self.u, self.y, self.v) = getInputOutputData(self.file_name, self.axis, self.t_start, self.t_stop) 50 | self.accept() 51 | else: 52 | self.printRangeError() 53 | 54 | def printRangeError(self): 55 | msg = QMessageBox() 56 | msg.setIcon(QMessageBox.Critical) 57 | msg.setWindowTitle("Error") 58 | msg.setText("Range is invalid") 59 | msg.exec_() 60 | 61 | def loadXData(self): 62 | if self.file_name: 63 | self.refreshInputOutputData(0) 64 | 65 | def loadYData(self): 66 | if self.file_name: 67 | self.refreshInputOutputData(1) 68 | 69 | def loadZData(self): 70 | if self.file_name: 71 | self.refreshInputOutputData(2) 72 | 73 | def refreshInputOutputData(self, axis=0): 74 | if self.file_name: 75 | self.axis = axis 76 | (self.t, self.u, self.y, self.v) = getInputOutputData(self.file_name, axis) 77 | self.plotInputOutput(redraw=True) 78 | 79 | def plotInputOutput(self, redraw=False): 80 | self.figure.clear() 81 | self.ax = self.figure.add_subplot(1,1,1) 82 | self.ax.plot(self.t, self.u, self.t, self.y) 83 | self.ax.set_title("Click and drag to select data range") 84 | self.ax.set_xlabel("Time (s)") 85 | self.ax.set_ylabel("Amplitude") 86 | self.ax.legend(["Input", "Output"]) 87 | 88 | self.span = SpanSelector(self.ax, self.onselect, 'horizontal', useblit=False, 89 | props=dict(alpha=0.2, facecolor='green'), interactive=True) 90 | 91 | self.t_start = self.t[0] 92 | self.t_stop = self.t[-1] 93 | 94 | self.canvas.mpl_connect('scroll_event', self.zoom_fun) 95 | 96 | self.canvas.draw() 97 | 98 | def onselect(self, xmin, xmax): 99 | indmin, indmax = np.searchsorted(self.t, (xmin, xmax)) 100 | indmax = min(len(self.t) - 1, indmax) 101 | indmin = min(indmin, indmax) 102 | 103 | self.t_start = self.t[indmin] 104 | self.t_stop = self.t[indmax] 105 | self.ax.set_xlim(self.t_start - 1.0, self.t_stop + 1.0) 106 | self.canvas.draw() 107 | 108 | def zoom_fun(self, event): 109 | base_scale = 1.1 110 | # get the current x and y limits 111 | cur_xlim = self.ax.get_xlim() 112 | cur_xrange = cur_xlim[1] - cur_xlim[0] 113 | xdata = event.xdata # get event x location 114 | if xdata is None or xdata < cur_xlim[0] or xdata > cur_xlim[1]: 115 | return 116 | 117 | if event.button == 'up': 118 | # deal with zoom in 119 | scale_factor = 1/base_scale 120 | elif event.button == 'down': 121 | # deal with zoom out 122 | scale_factor = base_scale 123 | else: 124 | # deal with something that should never happen 125 | scale_factor = 1 126 | # set new limits 127 | new_x_min = xdata - (xdata - cur_xlim[0])*scale_factor 128 | new_x_max = xdata + (xdata - new_x_min) / (xdata - cur_xlim[0]) * (cur_xlim[1] - xdata) 129 | 130 | new_x_min = max(new_x_min, self.t[0] - 1.0) 131 | new_x_max = min(new_x_max, self.t[-1] + 1.0) 132 | self.ax.set_xlim([new_x_min, new_x_max]) 133 | self.canvas.draw() 134 | -------------------------------------------------------------------------------- /autotune/logs/quadrotor_sitl_jmavsim.ulg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Auterion/Flight_Control_Prototyping_Scripts/6011414519b20478a011e79c620c25413b80a8cf/autotune/logs/quadrotor_sitl_jmavsim.ulg -------------------------------------------------------------------------------- /autotune/logs/quadrotor_x500.ulg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Auterion/Flight_Control_Prototyping_Scripts/6011414519b20478a011e79c620c25413b80a8cf/autotune/logs/quadrotor_x500.ulg -------------------------------------------------------------------------------- /autotune/logs/vtol_standard.ulg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Auterion/Flight_Control_Prototyping_Scripts/6011414519b20478a011e79c620c25413b80a8cf/autotune/logs/vtol_standard.ulg -------------------------------------------------------------------------------- /autotune/pid_design.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Copyright (c) 2021 PX4 Development Team 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions 7 | are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in 13 | the documentation and/or other materials provided with the 14 | distribution. 15 | 3. Neither the name PX4 nor the names of its contributors may be 16 | used to endorse or promote products derived from this software 17 | without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | 32 | File: pid_design.py 33 | Author: Mathieu Bresciani 34 | License: BSD 3-Clause 35 | Description: 36 | Design a PID controller based on the plant model 37 | and some design criteria 38 | """ 39 | 40 | import numpy as np 41 | 42 | def computePidGmvc(num, den, dt, sigma=0.1, delta=0.0, lbda=0.5): 43 | '''Compute a set of PID gains using General Minimum Variance Control law design 44 | 45 | Args: 46 | num: the coefficients of the numerator of the discrete-time plant transfer function 47 | den: the coefficients of the denominator of the discrete-time plant transfer function 48 | dt: the sampling time in seconds 49 | sigma: the desired closed-loop rise time in seconds 50 | delta: the damping index (between 0 and 2). 0 is critical damping, 1 is Butterworth 51 | lbda: the "detuning" coefficients. This affects the gain of the controller kc only. Increase to detune the controller. 52 | 53 | Raises: 54 | ValueError: if the is not a 2nd order system (2 poles) 55 | ValueError: if the plant has more than 2 zeros 56 | 57 | Returns: 58 | The PID gains in standard form: u = kc*[1 + ki*dt + kd/dt]*e 59 | kc: the controller gain 60 | ki: the integral gain (= 1/Ti) 61 | kd: the derivative gain (= Td) 62 | ''' 63 | if len(den) != 3: 64 | print("Only supports 2nd order system") 65 | return(0.0, 0.0, 0.0) 66 | 67 | if len(num) > 3: 68 | print("Cannot have more than 2 zeros") 69 | return(0.0, 0.0, 0.0) 70 | 71 | a1 = den[1] 72 | a2 = den[2] 73 | b0 = num[0] 74 | if len(num) > 1: 75 | b1 = num[1] 76 | else: 77 | b1 = 0 78 | if len(num) > 2: 79 | b2 = num[2] 80 | else: 81 | b2 = 0 82 | 83 | # Solve GMVC law (see derivation in pid_synthesis_symbolic.py) 84 | rho = dt/sigma 85 | mu = 0.25 * (1-delta) + 0.51 * delta 86 | p1 = -2*np.exp(-rho/(2*mu))*np.cos(np.sqrt(4*mu-1)*rho/(2*mu)) 87 | p2 = np.exp(-rho/mu) 88 | e1 = -a1 + p1 + 1 89 | f0 = -a1*e1 + a1 - a2 + e1 + p2 90 | f1 = a1*e1 - a2*e1 + a2 91 | f2 = a2*e1 92 | 93 | # Translate to PID gains 94 | nu = lbda + (e1 + 1)*(b0 + b1 + b2) 95 | kc = -(f1 + 2*f2)/nu 96 | ki = -(f0 + f1 + f2)/(dt*(f1 + 2*f2)) 97 | kd = -dt*f2/(f1 + 2*f2) 98 | 99 | return (kc, ki, kd) 100 | 101 | def gainsToNumDen(kc, ki, kd, dt): 102 | # use backwards Euler approximation for the derivative: s -> (z-1)/dt 103 | # and trapezoidal approximation for the integral: s -> 2/dt * (z-1)/(z+1) 104 | 105 | # Standard -> parallel form 106 | kI = kc * ki 107 | kD = kc * kd 108 | 109 | kIp = dt * kI / 2 110 | kDp = kD / dt 111 | 112 | b0 = kDp 113 | b1 = kc + kIp - 2*kDp 114 | b2 = -kc + kIp + kDp 115 | 116 | num = [b0, b1, b2] 117 | den = [1, -1, 0] 118 | return (num, den) 119 | 120 | def computePidDahlin(num, den, dt, rise_time=1.0): 121 | if len(den) != 3: 122 | print("Only supports 2nd order system") 123 | return(0.0, 0.0, 0.0) 124 | 125 | if len(num) > 3: 126 | print("Cannot have zeros") 127 | return(0.0, 0.0, 0.0) 128 | 129 | 130 | Q = 1.0 - np.exp(-dt / rise_time) 131 | a1 = den[1] 132 | a2 = den[2] 133 | b0 = num[0] 134 | 135 | if len(num) > 1: 136 | b1 = num[1] 137 | else: 138 | b1 = 0 139 | if len(num) > 2: 140 | b2 = num[2] 141 | else: 142 | b2 = 0 143 | b0 = b0 + b1 + b2 144 | 145 | kc = -(a1 + 2.0 * a2) * Q / b0 146 | 147 | Td = dt * a2 * Q / (kc * b0) 148 | Ti = -dt / (1.0 / (a1 + 2.0 * a2) + 1.0 + Td / dt) 149 | 150 | ki = 1.0 / Ti 151 | kd = Td 152 | 153 | return (kc, ki, kd) 154 | -------------------------------------------------------------------------------- /autotune/pid_synthesis_symbolic.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Copyright (c) 2021 PX4 Development Team 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions 7 | are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in 13 | the documentation and/or other materials provided with the 14 | distribution. 15 | 3. Neither the name PX4 nor the names of its contributors may be 16 | used to endorse or promote products derived from this software 17 | without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | 32 | File: pid_synthesis_symbolic.py 33 | Author: Mathieu Bresciani 34 | License: BSD 3-Clause 35 | Description: 36 | Derivation of a PID controller using the relationship between 37 | PID and Generalized Minimum Variance Control (GMVC) law 38 | 39 | Reference: 40 | T.Yamatoto, K.Fujii and M.Kaneda, Design and implementation of a self-tuning pid controller, 1998 41 | """ 42 | 43 | from sympy import * 44 | from sympy import linsolve 45 | 46 | z = Symbol("z", real=True) 47 | 48 | # Denominator coefficients 1 + a1*z^-1 + a2*z^-2 49 | a1 = Symbol("a1", real=True) 50 | a2 = Symbol("a2", real=True) 51 | 52 | # Controller 53 | f0 = Symbol("f0", real=True) 54 | f1 = Symbol("f1", real=True) 55 | f2 = Symbol("f2", real=True) 56 | e1 = Symbol("e1", real=True) 57 | e2 = Symbol("e2", real=True) 58 | 59 | p1 = Symbol("p1", real=True) 60 | p2 = Symbol("p2", real=True) 61 | 62 | km = 1 63 | 64 | P = 1 + p1 * z**-1 + p2 * z**-2 65 | delta = 1 - z**-1 66 | E = 1 + e1 * z**-1 67 | F = f0 + f1 * z**-1 + f2 * z**-2 68 | A = 1 + a1 * z**-1 + a2 * z**-2 69 | 70 | # Compute eq.11 and collect the factors of each negative power of z 71 | expanded = expand(delta * A * E + z**-(km +1) * F) 72 | collected = collect(expanded, z) 73 | 74 | # Compare with the the polynomial P to find e1 and f0 75 | res_e1 = solveset(collected.coeff(z, -1) - p1, e1) 76 | res_f0 = solveset(collected.coeff(z, -2) - p2, f0) 77 | 78 | # Solve f1 and f2 given that the coefficients of 79 | # z^-3 and z^-4 = 0 80 | res_f1 = solveset(collected.coeff(z, -3), f1) 81 | res_f2 = solveset(collected.coeff(z, -4), f2) 82 | 83 | # Numerator coefficients b0 + b1*z^-1 + b2*z^-2 84 | b0 = Symbol("b0", real=True) 85 | b1 = Symbol("b1", real=True) 86 | b2 = Symbol("b2", real=True) 87 | lbda = Symbol("lbda", real=True) 88 | B = b0 + b1 * z**-1 + b2 * z**-2 89 | res_nu = E.subs(z, 1) * B.subs(z, 1) + lbda 90 | 91 | # Compute kc, ki and kd from F and nu 92 | kc = Symbol("kc", real=True) 93 | KI = Symbol("KI", real=True) 94 | KD = Symbol("KD", real=True) 95 | dt = Symbol("dt", real=True) 96 | nu = Symbol("nu", real=True) 97 | # Parallel form is required to solve a linear set of equations 98 | C = kc + KI*dt + KD/dt - (kc + 2*KD/dt) * z**-1 + KD/dt * z**-2 99 | Eqns = [C.coeff(z, 0)-f0/nu, C.coeff(z, -1)-f1/nu, C.coeff(z, -2)-f2/nu] 100 | res = linsolve(Eqns, kc, KI, KD) 101 | # Transform results into standard form 102 | res_kc = res.args[0][0] 103 | res_ki = res.args[0][1]/res_kc 104 | res_kd = res.args[0][2]/res_kc 105 | 106 | print("# Parameters") 107 | print("sigma = 0.04 # desired rise time in seconds") 108 | print("delta = 1.0 # damping index, set between 0 and 2 (0 for binomial, 1 for Butterworth)") 109 | print("lbda = 0.5 # tuning parameter, increase to increase robustness") 110 | print("") 111 | print("# Algorithm") 112 | print("rho = dt/sigma") 113 | print("mu = 0.25 * (1-delta) + 0.51 * delta") 114 | print("p1 = -2*np.exp(-rho/(2*mu))*np.cos((np.sqrt(4*mu-1)*rho/(2*mu)))") 115 | print("p2 = np.exp(-rho/mu)") 116 | print("e1 = {}".format(res_e1.args[0])) 117 | print("f0 = {}".format(res_f0.args[0])) 118 | print("f1 = {}".format(res_f1.args[0])) 119 | print("f2 = {}".format(res_f2.args[0])) 120 | print("nu = {}".format(res_nu)) 121 | print("kc = {}".format(res_kc)) 122 | print("ki = {}".format(res_ki)) 123 | print("kd = {}".format(res_kd)) 124 | -------------------------------------------------------------------------------- /autotune/readme.md: -------------------------------------------------------------------------------- 1 | # Installation using a virtual environment 2 | ## Create a new venv 3 | ``` 4 | python3.9 -m venv virtualenv-test 5 | source virtualenv-test/bin/activate 6 | ``` 7 | 8 | ## Install the dependencies 9 | ``` 10 | pip3 install scipy pyulog control pyqt5 11 | ``` 12 | 13 | ## Start the GUI 14 | ``` 15 | python3 autotune.py 16 | ``` 17 | 18 | ![image](https://github.com/user-attachments/assets/fcdf5c25-d92d-4487-9736-e77f6576d180) 19 | 20 | -------------------------------------------------------------------------------- /autotune/simulated_autotune.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Copyright (c) 2021 PX4 Development Team 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions 7 | are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in 13 | the documentation and/or other materials provided with the 14 | distribution. 15 | 3. Neither the name PX4 nor the names of its contributors may be 16 | used to endorse or promote products derived from this software 17 | without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | 32 | File: simulated_autotune.py 33 | Author: Mathieu Bresciani 34 | License: BSD 3-Clause 35 | Description: 36 | Auto-tuning algorithm test on simulated 2nd order transfer function 37 | """ 38 | 39 | import numpy as np 40 | import matplotlib.pylab as plt 41 | from scipy import signal 42 | from arx_rls import ArxRls 43 | from pid_design import computePidGmvc 44 | 45 | def run(): 46 | # Generate 2nd order transfer function 47 | zeta = 0.2 # damping ratio 48 | f_n = 2.0 # natural frequency 49 | w_n = f_n * 2.0*np.pi 50 | num = [w_n**2] 51 | den = [1, 2.0*zeta*w_n, w_n**2] 52 | Gs = signal.TransferFunction(num, den) 53 | 54 | # Simulation parameters 55 | n_steps = 1000 56 | t = np.linspace(0, 5, n_steps) 57 | u = np.ones(n_steps) # input signal 58 | u[int(n_steps/2):-1] = -1 59 | 60 | # Simulate the output of the continuous-time system 61 | t, y, x = signal.lsim(Gs, U=u, T=t) 62 | dt = t[1] 63 | 64 | # Identification 65 | n = 2 # order of the denominator (a_1,...,a_n) 66 | m = 2 # order of the numerator (b_0,...,b_m) 67 | d = 1 68 | rls = ArxRls(n, m, d) 69 | for k in range(n_steps): 70 | rls.update(u[k], y[k]) 71 | theta_hat = rls._theta_hat 72 | 73 | # Construct discrete-time TF from vector of estimated parameters 74 | num = [theta_hat.item(i) for i in range(n, n+m+1)] # b0 .. bm 75 | den = [theta_hat.item(i) for i in range(0, n)] # a1 .. an 76 | den.insert(0, 1.0) # add 1 to get [1, a1, .., an] 77 | Gz = signal.TransferFunction(num, den, dt=dt) 78 | # TODO: add delay of d 79 | 80 | # Simulate the output and compare with the true system 81 | t, y_est = signal.dlsim(Gz, u, t=t) 82 | plt.plot(t, y, t, y_est) 83 | plt.legend(["True", "Estimated"]) 84 | plt.xlabel("Time (s)") 85 | plt.ylabel("Amplitude (-)") 86 | plt.show() 87 | 88 | # design controller 89 | (kc, ki, kd) = computePidGmvc(num, den, dt, sigma=0.1, delta=0.0, lbda=0.5) 90 | 91 | print("kc = {}, ki = {}, kd = {}\n".format(kc, ki, kd)) 92 | return 93 | 94 | if __name__ == '__main__': 95 | run() 96 | -------------------------------------------------------------------------------- /autotune/system_identification.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Copyright (c) 2021 PX4 Development Team 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions 7 | are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in 13 | the documentation and/or other materials provided with the 14 | distribution. 15 | 3. Neither the name PX4 nor the names of its contributors may be 16 | used to endorse or promote products derived from this software 17 | without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | 32 | File: system_identification.py 33 | Author: Mathieu Bresciani 34 | License: BSD 3-Clause 35 | Description: 36 | Class performing input and output signal pre-processing (e.g.: remove bias and 37 | unwanted frequencies using high and low-pass filters) and 38 | running a weighted RLS algorithm to identify an ARX parametric model 39 | """ 40 | 41 | import numpy as np 42 | import control as ctrl 43 | 44 | from arx_rls import ArxRls 45 | from scipy.optimize import lsq_linear, minimize 46 | 47 | class SystemIdentification(object): 48 | def __init__(self, n=2, m=2, d=1): 49 | self.n = n # order of the denominator (a_1,...,a_n) 50 | self.m = m # order of the numerator (b_0,...,b_m) 51 | self.d = d # number of delays 52 | self.forgetting_tc = 60.0 # forgetting factor for weighted RLS in seconds 53 | self.f_hp = 0.5 # high-pass filter cutoff frequency 54 | self.f_lp = 30.0 # low-pass filter cutoff frequency 55 | 56 | def run(self, t, u, y): 57 | n_steps = len(t) 58 | dt = t[1] - t[0] 59 | 60 | # High-pass filter parameters 61 | if self.f_hp > 0.0: 62 | tau_hp = 1/(2*np.pi*self.f_hp) 63 | alpha_hp = tau_hp/(tau_hp+dt) 64 | else: 65 | alpha_hp = 0.0 66 | 67 | u_hp = np.zeros(n_steps) 68 | y_hp = np.zeros(n_steps) 69 | u_hp[0] = u[0] 70 | y_hp[0] = y[0] 71 | 72 | # Low-pass filter parameters 73 | tau_lp = 1/(2*np.pi*self.f_lp) 74 | alpha_lp = tau_lp/(tau_lp+dt) 75 | u_lp = np.zeros(n_steps) 76 | y_lp = np.zeros(n_steps) 77 | u_lp[0] = u[0] 78 | y_lp[0] = y[0] 79 | 80 | a_coeffs = np.zeros((self.n, n_steps)) 81 | b_coeffs = np.zeros((self.m + 1, n_steps)) 82 | 83 | # Apply high and low-pass filters 84 | for k in range(n_steps): 85 | if k > 0: 86 | if alpha_hp > 0.0: 87 | u_hp[k] = alpha_hp * u_hp[k-1] + alpha_hp*(u[k] - u[k-1]) 88 | y_hp[k] = alpha_hp * y_hp[k-1] + alpha_hp*(y[k] - y[k-1]) 89 | else: 90 | u_hp[k] = u[k] 91 | y_hp[k] = y[k] 92 | 93 | u_lp[k] = alpha_lp * u_lp[k-1] + (1-alpha_lp)*u_hp[k] 94 | y_lp[k] = alpha_lp * y_lp[k-1] + (1-alpha_lp)*y_hp[k] 95 | 96 | 97 | use_rls = True 98 | if use_rls: 99 | # Identification 100 | rls = ArxRls(self.n, self.m, self.d, lbda=(1 - dt / self.forgetting_tc)) 101 | 102 | for k in range(n_steps): 103 | # Update model 104 | rls.update(u_lp[k], y_lp[k]) 105 | 106 | theta_hat = rls._theta_hat 107 | 108 | # Save for plotting 109 | for i in range(self.n): 110 | a_coeffs[i,k] = theta_hat[i] 111 | for i in range(self.m+1): 112 | b_coeffs[i,k] = theta_hat[i+self.n] 113 | 114 | else: # use LS 115 | # Build matrix of regressors 116 | A = np.zeros((n_steps, self.n+self.m+1)) 117 | for row in range(n_steps): 118 | for i in range(self.n): 119 | A[row,i] = -y_lp[row-(i+1)] 120 | for i in range(self.m+1): 121 | A[row,i+self.n] = u_lp[row-(self.d+i)] 122 | 123 | B = [y_lp[i] for i in range(n_steps)] # Measured values 124 | 125 | res = lsq_linear(A, B, lsmr_tol='auto', verbose=1) 126 | theta_hat = res.x 127 | 128 | # Refine model using output-error optimization 129 | J = lambda x: np.sum(np.power(abs(np.array(B)-self.simulateModel(x, u_lp, dt)), 2.0)) # cost function 130 | x0 = np.append(res.x, 0) # initial conditions 131 | res = minimize(J, x0, method='nelder-mead', options={'disp': True}) 132 | theta_hat = res.x 133 | 134 | for i in range(self.n): 135 | a_coeffs[i,-1] = theta_hat[i] 136 | for i in range(self.m+1): 137 | b_coeffs[i,-1] = theta_hat[i+self.n] 138 | 139 | self.theta_hat = theta_hat 140 | 141 | return (theta_hat, a_coeffs, b_coeffs) 142 | 143 | def simulateModel(self, x, u, dt): 144 | a_coeffs = np.ones(self.n+1) 145 | b_coeffs = np.zeros(self.m+1) 146 | 147 | for i in range(self.n): 148 | a_coeffs[i+1] = x[i] 149 | for i in range(self.m+1): 150 | b_coeffs[i] = x[i+self.n] 151 | 152 | delays = ctrl.TransferFunction([1], np.append([1], np.zeros(self.d)), dt, inputs='r', outputs='rd') 153 | plant = ctrl.TransferFunction(b_coeffs, a_coeffs, dt, inputs='rd', outputs='y') 154 | 155 | system = ctrl.interconnect([delays, plant], inputs='r', outputs='y') 156 | 157 | _, y = ctrl.forced_response(system, U=u) 158 | return y 159 | 160 | def getNum(self): 161 | num = [self.theta_hat.item(i) for i in range(self.n, self.n+self.m+1)] # b0 .. bm 162 | return num 163 | 164 | def getDen(self): 165 | den = [self.theta_hat.item(i) for i in range(0, self.n)] # a1 .. an 166 | den.insert(0, 1.0) # add 1 to get [1, a1, .., an] 167 | return den 168 | -------------------------------------------------------------------------------- /control_allocation/README.md: -------------------------------------------------------------------------------- 1 | # Multicopter control allocation 2 | The algorithm implements the following idea: the signals going to the actuators are computed by the multiplication of the control vector **m** (desired rotational and linear accelerations/torques) by the control allocation matrix **P**. Then, some constraints (e.g.: vertical thrust or yaw acceleration) are relxed to minimize the saturation of the actuators. 3 | In PX4, the actuator effectiveness matrix **B** is defined in the toml files by the geometry of the vehicle and the control allocation matrix **P** is computed in the python script during compilation and is given to the mixer (nothing different so far). 4 | 5 | 6 | 7 | Instead of decreasing the input and re-mixing several times as it is done at the moment, this algorithm makes use of the vectors given by the control allocation matrix **P** to directly manipulate the output vector. Those vectors are orthogonal (4D space) and give the direct information about how to change the outputs in order to modify an input. In the case of a symmetric quad, the result is the same as before: add the same value to the 4 motors to modify thrust without changing roll/pitch/yaw accelerations. 8 | This strategy gives a more general approach and gives a good base for 6D control allocation of fully actuated vehicles. 9 | 10 | ## Saturation minimization 11 | Given that each axes given by the columns of the control allocation are orthogonal, we only need to optimize a single variable at a time an the optimal gain ca be find in a few simple steps: 12 | 1. Find the gain that unsaturates the most saturated actuator 13 | 2. Apply the gain to unsaturate it 14 | 3. repeat 1. to find 15 | 4. compute the optimal gain 16 | 17 | The optimal gain that minimizes the saturations is given by 18 | 19 | 20 | 21 | By identification, we find that 22 | 23 | 24 | 25 | ## Axis prioritization 26 | The strategy used to prioritize the axes is to allocate the axes starting with the higher priority and ending with the lower priority. 27 | 28 | Three modes of prioritization are currently supported: 29 | 30 | **Normal (safe) mode** - Roll/Pitch accelerations are allocated, Z thrust is used to unsaturate the actuators reaching the higher saturation (not the lower saturation, no "boosting" in this mode), roll/pitch acceleration are reduced if needed to unsaturate, then yaw acceleration is allocated and reduced if needed to fit into the remaining space. 31 | 32 | **Airmode XY** - Roll/Pitch accelerations are allocated, Z thrust is used to unsaturate the actuators (Z thrust can be increased), then yaw acceleration is allocated and reduced if needed to fit into the remaining space. 33 | 34 | **Airmode XYZ** - Roll/Pitch/Yaw accelerations are allocated and Z thrust is used to unsaturate the actuators. 35 | 36 | ### When to use which mode? 37 | The normal mode is the default mode as it is the safest one and should be used for all the new platform bringups. 38 | 39 | The Airmode XY is useful for some fixedwings VTOLs (e.g.: quadplane) as it helps to maintain stability when the wing has lift but the control surfaces become uneffective. It can also be used on multicopters with weak yaw control. 40 | 41 | The Airmode XYZ is useful for racer quads that have good yaw authority and require full attitude control even at zero and full throttle. 42 | -------------------------------------------------------------------------------- /control_allocation/mixer_convergence_vtol.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from mixerlib import normal_mode, mix_forward_thrust_and_yaw 3 | 4 | import matplotlib.pylab as plt 5 | from matplotlib.patches import FancyArrowPatch 6 | from mpl_toolkits.mplot3d import proj3d 7 | 8 | class Arrow3D(FancyArrowPatch): 9 | def __init__(self, xs, ys, zs, *args, **kwargs): 10 | FancyArrowPatch.__init__(self, (0,0), (0,0), *args, **kwargs) 11 | self._verts3d = xs, ys, zs 12 | 13 | def draw(self, renderer): 14 | xs3d, ys3d, zs3d = self._verts3d 15 | xs, ys, zs = proj3d.proj_transform(xs3d, ys3d, zs3d, renderer.M) 16 | self.set_positions((xs[0],ys[0]),(xs[1],ys[1])) 17 | FancyArrowPatch.draw(self, renderer) 18 | 19 | def plot_arrow(start,end,figure_handle, axis_handle=None): 20 | style="Simple,head_length=15,head_width=15,tail_width=10" 21 | vec = Arrow3D([start[0], end[0]], [start[1], end[1]], [start[2], end[2]], arrowstyle=style) 22 | 23 | if axis_handle == None: 24 | axis_handle = figure_handle.add_subplot(111, projection='3d') 25 | 26 | axis_handle.add_artist(vec) 27 | 28 | return axis_handle 29 | 30 | def mix_hover_prio(controls, P): 31 | 32 | # extract roll, pitch and thrust Z setpoints 33 | m_sp = np.matrix([controls[0,0] ,controls[0,1],0, controls[0,4]]).T 34 | u, u_opt = normal_mode(m_sp, P, np.matrix([0,0,0]).T, np.matrix([1,1,1]).T) 35 | return [u, u_opt] 36 | 37 | def calculate_forward_thrust_limits(u_z): 38 | u_min = np.zeros(2) 39 | u_max = np.zeros(2) 40 | 41 | for index in range(2): 42 | # total thrust needs to be smaller than one in magnitude 43 | u_max[index] = np.sqrt(1.0 - u_z[index]**2) 44 | 45 | 46 | return np.matrix(u_min),np.matrix(u_max) 47 | 48 | 49 | # demanded torque / thrust vector 50 | torque_demanded = np.array([0, 0, 0]) 51 | thrust_demanded = np.array([1, -0.5]) 52 | 53 | torque_thurst_demanded = np.matrix(np.concatenate((torque_demanded, thrust_demanded), axis=None)) 54 | 55 | 56 | # vehicle configuration for convergence VTOL 57 | # Motor 1 -> front right motor 58 | # Motor 2 -> front left motor 59 | # Motor 3 -> back motor (non-tilting) 60 | 61 | # Actuator definition 62 | # Actuator 1 -> Force in X direction of motor 1 63 | # Actuator 2 -> Force in X direction of motor 2 64 | # Actuator 3 -> Force in Z direction of motor 1 65 | # Actuator 4 -> Force in Z direction of motor 2 66 | # Actuator 5 -> Force in Z direction of motor 3 67 | 68 | motor1_pos = np.array([0.1, 0.3,0]) 69 | motor2_pos = np.array([0.1, -0.3,0]) 70 | motor3_pos = np.array([-0.3, 0, 0]) 71 | 72 | # torque vector that one unit of the respective actuator would generate 73 | actuator1_torque = np.cross(motor1_pos, np.array([1, 0, 0])) 74 | actuator2_torque = np.cross(motor2_pos, np.array([1, 0, 0])) 75 | actuator3_torque = np.cross(motor1_pos, np.array([0, 0, -1])) 76 | actuator4_torque = np.cross(motor2_pos, np.array([0, 0, -1])) 77 | actuator5_torque = np.cross(motor3_pos, np.array([0, 0, -1])) 78 | 79 | 80 | # actuator effectiveness matrix 81 | B = np.matrix([ 82 | [actuator1_torque[0], actuator2_torque[0], actuator3_torque[0], actuator4_torque[0], actuator5_torque[0]], 83 | [actuator1_torque[1], actuator2_torque[1], actuator3_torque[1], actuator4_torque[1], actuator5_torque[1]], 84 | [actuator1_torque[2], actuator2_torque[2], actuator3_torque[2], actuator4_torque[2], actuator5_torque[2]], 85 | [1, 1, 0, 0, 0], 86 | [0, 0, -1, -1, -1]]) 87 | 88 | np.set_printoptions(precision=3) 89 | np.set_printoptions(suppress=True) 90 | 91 | P = np.linalg.pinv(B) 92 | 93 | # do some weird scaling 94 | max_val_rpy = np.max(P[:,0:3]) 95 | max_thr = np.max(P[:,3:5]) 96 | P[:,0:3] = P[:,0:3] / max_val_rpy 97 | P[:,3:5] = P[:,3:5] / max_thr 98 | 99 | # calculate mixer matrix only for actuating, roll, pitch and thrust in Z direction 100 | P_mc = P 101 | P_mc = np.delete(P_mc, [0,1],0) 102 | P_mc = np.delete(P_mc, [3],1) 103 | 104 | u_hover_orig, u_hover_opt = mix_hover_prio(torque_thurst_demanded, P_mc) 105 | 106 | # calculate forward thrust (x direction limits) 107 | thrust_x_min,thrust_x_max = calculate_forward_thrust_limits(u_hover_opt) 108 | 109 | # calculate mixer mixer which only takes into account forward thrust and yaw 110 | P_fx_yaw = P 111 | P_fx_yaw = np.delete(P_fx_yaw, [2,3,4],0) 112 | P_fx_yaw = np.delete(P_fx_yaw, [0,1,4],1) 113 | 114 | # compute forward thrust values, given the limits imposed by the other actuators already being mixed 115 | input_vector = np.matrix([torque_thurst_demanded[0,2], torque_thurst_demanded[0,3]]).T 116 | u_fx_orig, u_fx_opt = mix_forward_thrust_and_yaw(input_vector, P_fx_yaw, thrust_x_min.T, thrust_x_max.T) 117 | 118 | 119 | # create the thrust vector of each motor 120 | mot1_thrust = np.array([u_fx_opt[0], 0, u_hover_opt[0]]) 121 | mot2_thrust = np.array([u_fx_opt[1], 0, u_hover_opt[1]]) 122 | mot3_thrust = np.array([0, 0, u_hover_opt[2]]) 123 | 124 | 125 | # draw the results 126 | fig = plt.figure() 127 | ax = plot_arrow(motor1_pos, motor1_pos + mot1_thrust, fig) 128 | ax = plot_arrow(motor2_pos, motor2_pos + mot2_thrust, fig, ax) 129 | ax = plot_arrow(motor3_pos, motor3_pos + mot3_thrust, fig, ax) 130 | ax.plot3D([-1,1], [0,0], [0,0]) 131 | ax.set(xlim=(-1, 1), ylim=(-1, 1), zlim=(0,1)) 132 | plt.show() 133 | 134 | 135 | 136 | -------------------------------------------------------------------------------- /control_allocation/mixer_multirotor.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | File: mixer_multirotor.py 6 | Author: Mathieu Bresciani 7 | Email: brescianimathieu@gmail.com 8 | Github: https://github.com/bresch 9 | Description: 10 | """ 11 | 12 | import numpy as np 13 | import numpy.matlib 14 | 15 | from mixerlib import * 16 | 17 | # -------------------------------------------------- 18 | # -------------------------------------------------- 19 | 20 | # Normalized actuator effectiveness matrix 21 | # m = B * u 22 | 23 | # Normal quad x 24 | #B = np.matrix([ 25 | # [-1.0, 1.0, 1.0, -1.0], 26 | # [1.0, -1.0, 1.0, -1.0], 27 | # [1.0, 1.0, -1.0, -1.0], 28 | # [1.0, 1.0, 1.0, 1.0]]) 29 | 30 | # Wide front arms 31 | B = np.matrix([ 32 | [-1.0, 0.5, 1.0, -0.5], 33 | [1.0, -1.0, 1.0, -1.0], 34 | [1.0, 1.0, -1.0, -1.0], 35 | [1.0, 1.0, 1.0, 1.0]]) 36 | 37 | # Compute the control allocation matrix using the pseudo inverse of the actuator effectiveness matrix 38 | # u = P * m 39 | P = np.linalg.pinv(B) 40 | 41 | ################################## 42 | # quad x 43 | P = np.matrix([ 44 | [ -0.707107, 0.707107, 1.000000, 1.000000 ], 45 | [ 0.707107, -0.707107, 1.000000, 1.000000 ], 46 | [ 0.707107, 0.707107, -1.000000, 1.000000 ], 47 | [ -0.707107, -0.707107, -1.000000, 1.000000 ]]) 48 | B = np.linalg.pinv(P) 49 | ################################## 50 | 51 | # Desired accelerations (actuator controls) 52 | p_dot_sp = 2.2 # roll acceleration (p is the roll rate) 53 | q_dot_sp = -0.1 # pitch acceleration 54 | r_dot_sp = 0.03 # yaw acceleration 55 | T_sp = 1.0 # vertical thrust 56 | m_sp = np.matrix([p_dot_sp, q_dot_sp, r_dot_sp, T_sp]).T # Vector of desired "accelerations" 57 | 58 | # Airmode type (none/xy/xyz) 59 | airmode = "none" 60 | 61 | # Actuators output saturations 62 | u_max = 1.0 63 | u_min = 0.0 64 | 65 | if airmode == "none": 66 | (u, u_new) = normal_mode(m_sp, P, u_min, u_max) 67 | elif airmode == "xy": 68 | (u, u_new) = airmode_xy(m_sp, P, u_min, u_max) 69 | elif airmode == "xyz": 70 | (u, u_new) = airmode_xyz(m_sp, P, u_min, u_max) 71 | else: 72 | u = 0.0 73 | u_new = 0.0 74 | 75 | # Saturate the outputs between 0 and 1 76 | u_new_sat = np.maximum(u_new, np.matlib.zeros(u.size).T) 77 | u_new_sat = np.minimum(u_new_sat, np.matlib.ones(u.size).T) 78 | 79 | # Display some results 80 | print("u = {}\n".format(u)) 81 | print("u_new = {}\n".format(u_new)) 82 | print("u_new_sat = {}\n".format(u_new_sat)) 83 | print("Desired accelerations = {}\n".format(m_sp)) 84 | # Compute back the allocated accelerations 85 | m_new = B * u_new_sat 86 | print("Allocated accelerations = {}\n".format(m_new)) 87 | -------------------------------------------------------------------------------- /control_allocation/mixerlib.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | File: mixer_multirotor.py 6 | Author: Mathieu Bresciani 7 | Email: brescianimathieu@gmail.com 8 | Github: https://github.com/bresch 9 | Description: 10 | """ 11 | 12 | import numpy as np 13 | import numpy.matlib 14 | 15 | 16 | def compute_desaturation_gain(u, u_min, u_max, delta_u): 17 | # Computes the gain k by which delta_u has to be multiplied 18 | # in order to unsaturate the output that has the greatest saturation 19 | d_u_sat_plus = u_max - u 20 | d_u_sat_minus = u_min - u 21 | k = np.zeros(u.size*2) 22 | for i in range(u.size): 23 | if abs(delta_u[i]) < 0.000001: 24 | # avoid division by zero 25 | continue 26 | 27 | if d_u_sat_minus[i] > 0.0: 28 | k[2*i] = d_u_sat_minus[i] / delta_u[i] 29 | if d_u_sat_plus[i] < 0.0: 30 | k[2*i+1] = d_u_sat_plus[i] / delta_u[i] 31 | 32 | k_min = min(k) 33 | k_max = max(k) 34 | 35 | # Reduce the saturation as much as possible 36 | k = k_min + k_max 37 | return k 38 | 39 | 40 | def minimize_sat(u, u_min, u_max, delta_u): 41 | # Minimize the saturation of the actuators by 42 | # adding or subtracting a fraction of delta_u. 43 | # Delta_u is the vector that added to the output u, 44 | # modifies the thrust or angular acceleration on a 45 | # specific axis. 46 | # For example, if delta_u is given 47 | # to slide along the vertical thrust axis, the saturation will 48 | # be minimized by shifting the vertical thrust setpoint, 49 | # without changing the roll/pitch/yaw accelerations. 50 | k_1 = compute_desaturation_gain(u, u_min, u_max, delta_u) 51 | u_1 = u + k_1 * delta_u # Try to unsaturate 52 | k_2 = compute_desaturation_gain(u_1, u_min, u_max, delta_u) 53 | 54 | # Compute optimal gain that equilibrates the saturations 55 | k_opt = k_1 + 0.5 * k_2 56 | 57 | u_prime = u + k_opt * delta_u 58 | return u_prime 59 | 60 | def mix_yaw(m_sp, u, P, u_min, u_max): 61 | m_sp_yaw_only = np.matlib.zeros(m_sp.size).T 62 | m_sp_yaw_only[2, 0] = m_sp[2, 0] 63 | u_p = u + P * m_sp_yaw_only 64 | 65 | # Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch), 66 | # and allow some yaw response at maximum thrust 67 | u_r_dot = P[:,2] 68 | u_pp = minimize_sat(u_p, u_min, u_max+0.15, u_r_dot) 69 | u_T = P[:, 3] 70 | u_ppp = minimize_sat(u_pp, -1000, u_max, u_T) 71 | return u_ppp 72 | 73 | def airmode_xy(m_sp, P, u_min, u_max): 74 | # Mix without yaw 75 | m_sp_no_yaw = m_sp.copy() 76 | m_sp_no_yaw[2, 0] = 0.0 77 | u = P * m_sp_no_yaw 78 | 79 | # Use thrust to unsaturate the outputs if needed 80 | u_T = P[:, 3] 81 | u_prime = minimize_sat(u, u_min, u_max, u_T) 82 | 83 | # Mix yaw axis independently 84 | u_final = mix_yaw(m_sp, u_prime, P, u_min, u_max) 85 | 86 | return (u, u_final) 87 | 88 | 89 | def airmode_xyz(m_sp, P, u_min, u_max): 90 | # Mix with yaw 91 | u = P * m_sp 92 | 93 | # Use thrust to unsaturate the outputs if needed 94 | u_T = P[:, 3] 95 | u_prime = minimize_sat(u, u_min, u_max, u_T) 96 | return (u, u_prime) 97 | 98 | 99 | def normal_mode(m_sp, P, u_min, u_max): 100 | # Mix without yaw 101 | m_sp_no_yaw = m_sp.copy() 102 | m_sp_no_yaw[2, 0] = 0.0 103 | u = P * m_sp_no_yaw 104 | 105 | # Use thrust to unsaturate the outputs if needed 106 | # by reducing the thrust only 107 | u_T = P[:, 3] 108 | u_prime = minimize_sat(u, u_min, u_max, u_T) 109 | if (u_prime > (u)).any(): 110 | u_prime = u 111 | 112 | # Reduce roll/pitch acceleration if needed to unsaturate 113 | u_p_dot = P[:, 0] 114 | u_p2 = minimize_sat(u_prime, u_min, u_max, u_p_dot) 115 | u_q_dot = P[:, 1] 116 | u_p3 = minimize_sat(u_p2, u_min, u_max, u_q_dot) 117 | 118 | # Mix yaw axis independently 119 | u_final = mix_yaw(m_sp, u_p3, P, u_min, u_max) 120 | return (u, u_final) 121 | 122 | def mix_forward_thrust_and_yaw(m_sp, P, u_min, u_max): 123 | # mix everything 124 | u = P * m_sp 125 | 126 | # use yaw to unsaturate 127 | u_T = P[:, 0] 128 | 129 | u_prime = minimize_sat(u, u_min, u_max, u_T) 130 | 131 | # use forward thrust to desaturate 132 | u_T = P[:, 1] 133 | 134 | u_final = minimize_sat(u_prime, u_min, u_max, u_T) 135 | 136 | return u,u_final 137 | -------------------------------------------------------------------------------- /drag_fusion_tuning/drag_fusion_symbolic.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Copyright (c) 2022 PX4 Development Team 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions 7 | are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in 13 | the documentation and/or other materials provided with the 14 | distribution. 15 | 3. Neither the name PX4 nor the names of its contributors may be 16 | used to endorse or promote products derived from this software 17 | without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | 32 | File: frag_fusion_symbolic.py 33 | Author: Mathieu Bresciani 34 | License: BSD 3-Clause 35 | Description: 36 | """ 37 | 38 | from sympy import * 39 | 40 | V = Symbol("V", real=True) 41 | rho = Symbol("rho", real=True) 42 | rho_n = Symbol("rho_n", real=True) 43 | Mcoef = Symbol("Mcoef", real=True) 44 | Bcoef = Symbol("Bcoef", real=True) 45 | a = Symbol("a", real=True) 46 | 47 | f1 = 0.5 * rho / Bcoef * V**2 + rho_n * Mcoef * V - a 48 | 49 | print("If Bcoef > 0 and Mcoef > 0") 50 | print("V =") 51 | res_V = solve(f1, V) 52 | res_V = res_V[0] 53 | pprint(res_V) 54 | print("a_pred =") 55 | pprint(solve(f1, a)) 56 | -------------------------------------------------------------------------------- /drag_fusion_tuning/drag_replay.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Copyright (c) 2022 PX4 Development Team 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions 7 | are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in 13 | the documentation and/or other materials provided with the 14 | distribution. 15 | 3. Neither the name PX4 nor the names of its contributors may be 16 | used to endorse or promote products derived from this software 17 | without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | 32 | File: drag_replay.py 33 | Author: Mathieu Bresciani 34 | License: BSD 3-Clause 35 | Description: 36 | Find the best ballistic and momentum drag coefficients for wind estimation 37 | using EKF2 replay data. 38 | NOTE: this script currently assumes no wind. 39 | """ 40 | 41 | import matplotlib.pylab as plt 42 | from pyulog import ULog 43 | from pyulog.px4 import PX4ULog 44 | import numpy as np 45 | import quaternion 46 | from scipy import optimize 47 | 48 | def getAllData(logfile): 49 | log = ULog(logfile) 50 | 51 | v_local = np.matrix([getData(log, 'vehicle_local_position', 'vx'), 52 | getData(log, 'vehicle_local_position', 'vy'), 53 | getData(log, 'vehicle_local_position', 'vz')]) 54 | 55 | t_v_local = ms2s(getData(log, 'vehicle_local_position', 'timestamp')) 56 | 57 | accel = np.matrix([getData(log, 'sensor_combined', 'accelerometer_m_s2[0]'), 58 | getData(log, 'sensor_combined', 'accelerometer_m_s2[1]'), 59 | getData(log, 'sensor_combined', 'accelerometer_m_s2[2]')]) 60 | t_accel = ms2s(getData(log, 'sensor_combined', 'timestamp')) 61 | 62 | q = np.matrix([getData(log, 'vehicle_attitude', 'q[0]'), 63 | getData(log, 'vehicle_attitude', 'q[1]'), 64 | getData(log, 'vehicle_attitude', 'q[2]'), 65 | getData(log, 'vehicle_attitude', 'q[3]')]) 66 | t_q = ms2s(getData(log, 'vehicle_attitude', 'timestamp')) 67 | 68 | dist_bottom = getData(log, 'vehicle_local_position', 'dist_bottom') 69 | t_dist_bottom = ms2s(getData(log, 'vehicle_local_position', 'timestamp')) 70 | 71 | (t_aligned, v_body_aligned, accel_aligned) = alignData(t_v_local, v_local, t_accel, accel, t_q, q, t_dist_bottom, dist_bottom) 72 | 73 | t_aligned -= t_aligned[0] 74 | 75 | return (t_aligned, v_body_aligned, accel_aligned) 76 | 77 | def alignData(t_v, v_local, t_accel, accel, t_q, q, t_dist_bottom, dist_bottom): 78 | len_accel = len(t_accel) 79 | len_q = len(t_q) 80 | len_db = len(t_dist_bottom) 81 | i_a = 0 82 | i_q = 0 83 | i_db = 0 84 | v_body_aligned = np.empty((3,0)) 85 | accel_aligned = np.empty((3,0)) 86 | t_aligned = [] 87 | 88 | for i_v in range(len(t_v)): 89 | t = t_v[i_v] 90 | accel_sum = np.zeros((3,1)) 91 | accel_count = 0 92 | while t_accel[i_a] < t and i_a < len_accel-1: 93 | accel_sum += accel[:, i_a] # Integrate accel samples between 2 velocity samples 94 | accel_count += 1 95 | i_a += 1 96 | while t_q[i_q] < t and i_q < len_q-1: 97 | i_q += 1 98 | while t_dist_bottom[i_db] < t and i_db < len_db-1: 99 | i_db += 1 100 | 101 | # Only use in air data 102 | if dist_bottom[i_db] < 1.0 or accel_count == 0: 103 | continue 104 | 105 | qk = np.quaternion(q[0, i_q],q[1, i_q],q[2, i_q],q[3, i_q]) 106 | q_vl = np.quaternion(0, v_local[0, i_v], v_local[1, i_v], v_local[2, i_v]) 107 | q_vb = qk.conjugate() * q_vl * qk # Get velocity in body frame 108 | vb = quaternion.as_float_array(q_vb)[1:4] 109 | 110 | v_body_aligned = np.append(v_body_aligned, [[vb[0]], [vb[1]], [vb[2]]], axis=1) 111 | accel_aligned = np.append(accel_aligned, accel_sum / accel_count, axis=1) 112 | t_aligned.append(t) 113 | 114 | return (t_aligned, v_body_aligned, np.asarray(accel_aligned)) 115 | 116 | def getData(log, topic_name, variable_name, instance=0): 117 | variable_data = np.array([]) 118 | for elem in log.data_list: 119 | if elem.name == topic_name: 120 | if instance == elem.multi_id: 121 | variable_data = elem.data[variable_name] 122 | break 123 | 124 | return variable_data 125 | 126 | def ms2s(time_ms): 127 | return time_ms * 1e-6 128 | 129 | def run(logfile): 130 | (t, v_body, a_body) = getAllData(logfile) 131 | 132 | rho = 1.15 # air densitiy 133 | rho15 = 1.225 # air density at 15 degC 134 | 135 | # x[0]: momentum drag, scales with v 136 | # x[1]: inverse of ballistic coefficient (X body axis), scales with v^2 137 | # x[1]: inverse of ballistic coefficient (Y body axis), scales with v^2 138 | predict_acc_x = lambda x: -v_body[0] * x[0] - 0.5 * rho * v_body[0]**2 * np.sign(v_body[0]) * x[1] 139 | predict_acc_y = lambda x: -v_body[1] * x[0] - 0.5 * rho * v_body[1]**2 * np.sign(v_body[1]) * x[2] 140 | 141 | J = lambda x: np.sum(np.power(abs(a_body[0]-predict_acc_x(x)), 2.0) + np.power(abs(a_body[1]-predict_acc_y(x)), 2.0)) # cost function 142 | 143 | x0 = [0.15, 1/100, 1/100] # initial conditions 144 | res = optimize.minimize(J, x0, method='nelder-mead', bounds=[(0,1),(0,10),(0,10)], options={'disp': True}) 145 | 146 | # Convert results to parameters 147 | innov_var = J(res.x) / (len(v_body[0]) + len(v_body[1])) 148 | mcoef = res.x[0] / np.sqrt(rho / rho15) 149 | 150 | bcoef_x = 0.0 151 | bcoef_y = 0.0 152 | 153 | if res.x[1] > 1/200: 154 | bcoef_x = 1/res.x[1] 155 | 156 | if res.x[2] > 1/200: 157 | bcoef_y = 1/res.x[2] 158 | 159 | print(f"param set EKF2_BCOEF_X {bcoef_x:.1f}") 160 | print(f"param set EKF2_BCOEF_Y {bcoef_y:.1f}") 161 | print(f"param set EKF2_MCOEF {mcoef:.2f}") 162 | print(f"/!\EXPERIMENTAL param set EKF2_DRAG_NOISE {innov_var:.2f}") 163 | 164 | # Plot data 165 | plt.figure(1) 166 | plt.suptitle(logfile.split('/')[-1]) 167 | ax1 = plt.subplot(2, 1, 1) 168 | ax1.plot(t, v_body[0]) 169 | ax1.plot(t, v_body[1]) 170 | ax1.set_xlabel("time (s)") 171 | ax1.set_ylabel("velocity (m/s)") 172 | ax1.legend(["forward", "right"]) 173 | 174 | ax2 = plt.subplot(2, 1, 2, sharex=ax1) 175 | ax2.set_title(f"BCoef_x = {bcoef_x:.1f}, BCoef_y = {bcoef_y:.1f}, MCoef = {mcoef:.4f}", loc="right") 176 | ax2.plot(t, a_body[0]) 177 | ax2.plot(t, a_body[1]) 178 | ax2.plot(t, predict_acc_x(res.x)) 179 | ax2.plot(t, predict_acc_y(res.x)) 180 | ax2.set_xlabel("time (s)") 181 | ax2.set_ylabel("acceleration (m/s^2)") 182 | ax2.legend(["meas_forward", "meas_right", "predicted_forward", "predicted_right"]) 183 | plt.show() 184 | 185 | if __name__ == '__main__': 186 | import os 187 | import argparse 188 | 189 | # Get the path of this script (without file name) 190 | script_path = os.path.split(os.path.realpath(__file__))[0] 191 | 192 | # Parse arguments 193 | parser = argparse.ArgumentParser( 194 | description='Estimate mag biases from ULog file') 195 | 196 | # Provide parameter file path and name 197 | parser.add_argument('logfile', help='Full ulog file path, name and extension', type=str) 198 | args = parser.parse_args() 199 | 200 | logfile = os.path.abspath(args.logfile) # Convert to absolute path 201 | 202 | run(logfile) 203 | -------------------------------------------------------------------------------- /drag_fusion_tuning/readme.md: -------------------------------------------------------------------------------- 1 | # PX4 Drag fusion parameter tuning algorithm 2 | In PX4, drag fusion can be enabled in order to estimate the wind when flying a multirotor, assuming that the body vertical acceleration is produced by the rotors and that the lateral forces are produced by drag. 3 | The model assumes a combination of: 4 | 1. momentum drag: created by the rotors changing the direction of the incoming air, linear to the relative airspeed. Parameter `EKF2_MCOEF` 5 | 2. bluff body drag: created by the wetted area of the aircraft, quadratic to the relative airspeed. Parameters `EKF2_BCOEF_X` and `EKF2_BCOEF_Y` 6 | 7 | The python script was created to automate the tuning of the aforementioned parameters using flight test data. 8 | 9 | ## How to use this script 10 | First, a flight log with enough information is required in order to accurately estimate the parameters. 11 | The best way to do this is to fly the drone in altitude mode, accelerate to a moderate-high speed and let the drone slow-down by its own drag. 12 | Repeat the same maneuver in all directions and several times to obtain a good dataset. 13 | 14 | /!\ NOTE: the current state of this script assumes no wind. Some modifications are required to estimate both the wind and the parameters at the same time. 15 | 16 | Then, install the required python packages: 17 | ``` 18 | pip install -r requirements.txt 19 | ``` 20 | and run the script and give it the log file as an argument: 21 | ``` 22 | python drag_replay.py 23 | ``` 24 | 25 | The estimated parameters are displayed in the console and the fit quality is shown in a plot: 26 | ``` 27 | param set EKF2_BCOEF_X 0.0 28 | param set EKF2_BCOEF_Y 62.1 29 | param set EKF2_MCOEF 0.16 30 | /!\EXPERIMENTAL param set EKF2_DRAG_NOISE 0.31 31 | ``` 32 | ![DeepinScreenshot_matplotlib_20220329100027](https://user-images.githubusercontent.com/14822839/160563024-efddd100-d7db-46f7-8676-cf4296e9f737.png) 33 | -------------------------------------------------------------------------------- /drag_fusion_tuning/requirements.txt: -------------------------------------------------------------------------------- 1 | matplotlib==3.5.1 2 | numpy==1.22.2 3 | pyulog==0.9.0 4 | quaternion==3.5.2.post4 5 | scipy==1.8.0 6 | sympy==1.10.1 7 | -------------------------------------------------------------------------------- /filters/digital_filter_compare.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | File: digital_filter_compare.py 6 | Author: Mathieu Bresciani 7 | Email: brescianimathieu@gmail.com 8 | Github: https://github.com/bresch 9 | Description: 10 | Plot the frequency and step responses of several digital filters 11 | """ 12 | 13 | import numpy as np 14 | from scipy import signal 15 | from matplotlib import pyplot as plt 16 | 17 | frequencies = [] 18 | amplitudes = [] 19 | times = [] 20 | step_responses = [] 21 | group_delays = [] 22 | names = [] 23 | 24 | def addFilter(b, a, sampling_freq, name=""): 25 | w, h = signal.freqz(b, a, fs=sampling_freq) 26 | t, y = signal.dstep((b, a, 1.0/fs)) 27 | w_gd, gd = signal.group_delay((b, a)) 28 | 29 | frequencies.append(w) 30 | amplitudes.append(h) 31 | times.append(t) 32 | step_responses.append(y) 33 | group_delays.append(gd) 34 | names.append(name) 35 | 36 | def plotFilters(): 37 | plotBode() 38 | plotStep() 39 | 40 | def plotBode(): 41 | fig, (ax1, ax2, ax3) = plt.subplots(3, 1, sharex=True) 42 | 43 | n_filters = len(frequencies) 44 | for n in range(n_filters): 45 | ax1.semilogx(frequencies[n], 20 * np.log10(abs(amplitudes[n]))) 46 | angles = np.rad2deg(np.unwrap(np.angle(amplitudes[n]))) 47 | ax2.semilogx(frequencies[n], angles) 48 | group_delay_ms = group_delays[n] / fs * 1e3 49 | ax3.semilogx(frequencies[n], group_delay_ms) 50 | 51 | ax1.set_title('Digital filter frequency response') 52 | ax1.set_ylabel('Amplitude (dB)') 53 | ax2.set_ylabel('Angle (degrees)') 54 | ax3.set_ylabel('Group delay (ms)') 55 | ax3.set_xlabel('Frequency (Hz)') 56 | ax1.legend(names) 57 | ax1.grid() 58 | ax2.grid() 59 | ax2.axis('tight') 60 | plt.show() 61 | 62 | def plotStep(): 63 | n_filters = len(times) 64 | for n in range(n_filters): 65 | plt.plot(times[n], np.squeeze(step_responses[n])) 66 | 67 | plt.title('Digital filter frequency response') 68 | plt.xlabel('Time (s)') 69 | plt.ylabel('Amplitude (-)') 70 | plt.legend(names) 71 | plt.grid() 72 | plt.show() 73 | 74 | def create1stOrderButterworthLpf(fc, fs): 75 | gamma = np.tan(np.pi * fc / fs) 76 | D = gamma + 1.0 77 | b = [gamma / D, gamma / D] 78 | a = [1.0, (gamma - 1.0) / D] 79 | name = createName("Lpf-1-Butter", fc) 80 | return b, a, name 81 | 82 | def create2ndOrderButterworthLpf(fc, fs): 83 | gamma = np.tan(np.pi * fc / fs) 84 | gamma2 = gamma**2 85 | D = gamma2 + np.sqrt(2.0) * gamma + 1.0 86 | b0_prime = gamma2 87 | b1_prime = 2.0 * b0_prime 88 | b2_prime = b0_prime 89 | a0_prime = D 90 | a1_prime = 2.0 * (gamma2 - 1.0) 91 | a2_prime = gamma2 - np.sqrt(2.0) * gamma + 1.0 92 | 93 | b = [b0_prime, b1_prime, b2_prime] / D 94 | a = [a0_prime, a1_prime, a2_prime] / D 95 | name = createName("Lpf-2-Butter", fc) 96 | return b, a, name 97 | 98 | def createLpf2p(fc, fs): 99 | fr = fs / fc 100 | ohm = np.tan(np.pi / fr) 101 | c = 1.0 + 2.0 * np.cos(np.pi / 4.0) * ohm + ohm**2 102 | 103 | b0 = ohm**2 / c 104 | b1 = 2.0 * b0 105 | b2 = b0 106 | a0 = 1.0 107 | a1 = 2.0 * (ohm**2 - 1.0) / c 108 | a2 = (1.0 - 2.0 * np.cos(np.pi / 4.0) * ohm + ohm**2) / c 109 | 110 | b = [b0, b1, b2] 111 | a = [a0, a1, a2] 112 | name = createName("Lpf-2-PX4", fc) 113 | return b, a, name 114 | 115 | def create2ndOrderNotch(fc, bw, fs): 116 | alpha = np.tan(np.pi * bw / fs) 117 | beta = -np.cos(2.0 * np.pi * fc / fs) 118 | D = alpha + 1.0 119 | b0_prime = 1.0 120 | b1_prime = 2.0 * beta 121 | b2_prime = 1.0 122 | a0_prime = D 123 | a1_prime = b1_prime 124 | a2_prime = 1.0 - alpha 125 | 126 | b = [b0_prime / D, b1_prime / D, b2_prime / D] 127 | a = [a0_prime / D, a1_prime / D, a2_prime / D] 128 | name = createName("Notch-2", fc, bw) 129 | return b, a, name 130 | 131 | def create2ndOrderButterworthBandStop(fc, bw, fs): 132 | gamma = np.tan(np.pi * fc / fs) 133 | gamma2 = gamma**2 134 | D = (1.0 + gamma2) * fc + gamma * bw 135 | b0_prime = fc * (gamma2 + 1.0) 136 | b1_prime = 2.0 * fc * (gamma2 - 1.0) 137 | b2_prime = b0_prime 138 | a0_prime = D 139 | a1_prime = b1_prime 140 | a2_prime = (1.0 + gamma2) * fc - gamma * bw 141 | 142 | b = [b0_prime, b1_prime, b2_prime] / D 143 | a = [a0_prime, a1_prime, a2_prime] / D 144 | name = createName("BStop-2-Butter", fc, bw) 145 | return b, a, name 146 | 147 | def create2ndOrderLpf(fc, zeta, fs): 148 | T = 1.0 / fs 149 | wn = 2.0 * np.pi * fc 150 | K = wn / np.tan(wn * T / 2.0) 151 | K2 = K**2 152 | b2a = wn**2 153 | a0a = 1.0 154 | a1a = 2.0 * zeta * wn 155 | a2a = wn**2 156 | D = a0a*K2 + a1a*K + a2a 157 | b0_prime = b2a 158 | b1_prime = 2.0 * b2a 159 | b2_prime = b2a 160 | a0_prime = D 161 | a1_prime = 2.0 * a2a - 2.0*K2 162 | a2_prime = a0a*K2 - a1a*K + a2a 163 | 164 | b = [b0_prime / D, b1_prime / D, b2_prime / D] 165 | a = [a0_prime / D, a1_prime / D, a2_prime / D] 166 | name = createName("Lpf-2-damp", fc) 167 | return b, a, name 168 | 169 | def create2ndOrderCriticallyDamped(fc, fs): 170 | wn = 2.0 * np.pi * fc 171 | K = 2.0 * fs 172 | K2 = K**2 173 | b2a = wn**2 174 | a1a = 2.0 * wn 175 | a2a = wn**2 176 | D = K2 + a1a*K + a2a 177 | b0_prime = b2a 178 | b1_prime = 2.0 * b2a 179 | b2_prime = b2a 180 | a0_prime = D 181 | a1_prime = 2.0 * a2a - 2.0*K2 182 | a2_prime = K2 - a1a*K + a2a 183 | 184 | b = [b0_prime / D, b1_prime / D, b2_prime / D] 185 | a = [a0_prime / D, a1_prime / D, a2_prime / D] 186 | name = createName("Lpf-2-crit-damp", fc) 187 | return b, a, name 188 | 189 | def create1stOrderLpf(fc, fs): 190 | dt = 1.0 / fs 191 | tau = 1 / (2.0 * np.pi * fc) 192 | alpha = dt / (tau + dt) 193 | b0 = alpha 194 | a0 = 1.0 195 | a1 = alpha - 1.0 196 | b = [b0] 197 | a = [a0, a1] 198 | name = createName("Lpf-1-alpha", fc) 199 | return b, a, name 200 | 201 | def create1stOrderHpf(fc, fs): 202 | dt = 1.0 / fs 203 | tau = 1 / (2.0 * np.pi * fc) 204 | alpha = fs / (2.0 * np.pi * fc + fs) 205 | b0 = 1.0 206 | b1 = -1.0 207 | a0 = 1.0 / alpha 208 | a1 = -1.0 209 | b = [b0, b1] 210 | a = [a0, a1] 211 | name = createName("Hpf-1-alpha", fc) 212 | return b, a, name 213 | 214 | def create1stOrderButterworthHpf(fc, fs): 215 | dt = 1.0 / fs 216 | gamma = np.tan(np.pi * fc / fs) 217 | b0 = 1.0 218 | b1 = -1.0 219 | a0 = gamma + 1.0 220 | a1 = gamma - 1.0 221 | b = [b0, b1] 222 | a = [a0, a1] 223 | name = createName("Hpf-1-Butter", fc) 224 | return b, a, name 225 | 226 | def create2ndOrderButterworthHpf(fc, fs): 227 | # Butterworth 228 | dt = 1.0 / fs 229 | gamma = np.tan(np.pi * fc / fs) 230 | gamma2 = gamma**2 231 | D = gamma2 + np.sqrt(2) * gamma + 1.0 232 | b0_prime = 1.0 233 | b1_prime = -2.0 234 | b2_prime = 1.0 235 | a0_prime = D 236 | a1_prime = 2.0 * (gamma2 - 1.0) 237 | a2_prime = gamma2 - np.sqrt(2) * gamma + 1.0 238 | 239 | b = [b0_prime, b1_prime, b2_prime] / D 240 | a = [a0_prime, a1_prime, a2_prime] / D 241 | name = createName("Hpf-2-Butter", fc) 242 | return b, a, name 243 | 244 | def createName(prefix, fc, bw=0.0): 245 | name = prefix + " (fc = {}".format(fc) 246 | if bw > 0.1: 247 | name += " bw = {}".format(bw) 248 | name += ")".format(bw) 249 | return name 250 | 251 | if __name__ == '__main__': 252 | fs = 1000.0 253 | cutoff = 80.0 254 | bandwidth = 30.0 255 | 256 | b, a, name = create1stOrderButterworthLpf(cutoff, fs) 257 | addFilter(b, a, fs, name) 258 | b, a, name = create2ndOrderButterworthLpf(cutoff, fs) 259 | addFilter(b, a, fs, name) 260 | b, a, name = create2ndOrderNotch(cutoff, bandwidth, fs) 261 | addFilter(b, a, fs, name) 262 | b, a, name = create2ndOrderButterworthBandStop(cutoff, bandwidth, fs) 263 | addFilter(b, a, fs, name) 264 | b, a, name = createLpf2p(cutoff, fs) 265 | addFilter(b, a, fs, name) 266 | b, a, name = create2ndOrderLpf(fc=cutoff, zeta=1.0, fs=fs) 267 | addFilter(b, a, fs, name) 268 | b, a, name = create2ndOrderCriticallyDamped(cutoff, fs=fs) 269 | addFilter(b, a, fs, name) 270 | b, a, name = create1stOrderLpf(cutoff, fs=fs) 271 | addFilter(b, a, fs, name) 272 | b, a, name = create1stOrderHpf(5.0, fs=fs) 273 | addFilter(b, a, fs, name) 274 | b, a, name = create1stOrderButterworthHpf(5.0, fs=fs) 275 | addFilter(b, a, fs, name) 276 | b, a, name = create2ndOrderButterworthHpf(5.0, fs=fs) 277 | addFilter(b, a, fs, name) 278 | 279 | plotFilters() 280 | -------------------------------------------------------------------------------- /hover_thrust_estimator/HoverThrEstimator.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | File: HoverThrEstimator.py 6 | Author: Mathieu Bresciani 7 | Email: brescianimathieu@gmail.com 8 | Github: https://github.com/bresch 9 | Description: 10 | 1-state hover thrust estimator 11 | state: hover thrust (Th) 12 | The measurement is the vertical acceleration and the current 13 | thrust (T[k]) is used in the measurement model. 14 | The state is noise driven: Transition matrix A = 1 15 | x[k+1] = Ax[k] + v with v ~ N(0, Q) 16 | y[k] = h(u, x) + w with w ~ N(0, R) 17 | Where the measurement model and corresponding Jocobian are: 18 | h(u, x) = g * T[k] / Th[k] - g 19 | H[k] = -g * T[k] / Th[k]**2 20 | """ 21 | 22 | from numpy import * 23 | import sys 24 | import math 25 | import matplotlib.pylab as plt 26 | 27 | innov_sq_length = 10 28 | FLT_EPSILON = sys.float_info.epsilon 29 | NAN = float('nan') 30 | verbose = True; 31 | 32 | if verbose: 33 | def verboseprint(*args): 34 | # Print each argument separately so caller doesn't need to 35 | # stuff everything to be printed into a single string 36 | for arg in args: 37 | print(arg) 38 | print() 39 | else: 40 | verboseprint = lambda *a: None # do-nothing function 41 | 42 | class HoverThrEstimator(object): 43 | 44 | def setState(self, hover_thr): 45 | self._hover_thr = hover_thr 46 | 47 | def setStateVar(self, hover_thr_var): 48 | self._P = hover_thr_var 49 | 50 | def setProcessVar(self, hover_thr_process_noise_var): 51 | self._Q = hover_thr_process_noise_var 52 | 53 | def setMeasVar(self, accel_var): 54 | self._R = accel_var 55 | 56 | def setMeasVarCoeff(self, coeff): 57 | self._R_gain = coeff 58 | 59 | def resetInnovSq(self): 60 | self._innov_sq = 0.0 61 | self._C = 0.0 62 | self._nb_innov_sq = 0 63 | 64 | def setInnovGateSize(self, gate_size): 65 | self._innov_gate_size = gate_size 66 | 67 | def __init__(self, hover_thr): 68 | self.setState(hover_thr) 69 | self.setStateVar(0.05) 70 | self.setProcessVar(0.3**2) 71 | self.setMeasVar(0.02) 72 | self.setMeasVarCoeff(1.0) 73 | self.resetInnovSq() 74 | self.setInnovGateSize(3.0) 75 | self._dt = 1e-3 76 | self._noise_learning_time_constant = 2.0 77 | self._lpf_time_constant = 1.0 78 | self._residual_lpf = 0.0 79 | self._innov_test_ratio_signed_lpf = 0.0 80 | 81 | def predict(self, dt): 82 | # State is constant 83 | # Predict error covariance only 84 | self._P += self._Q * dt * dt 85 | self._dt = dt 86 | 87 | def fuseAccZ(self, acc_z, thrust): 88 | H = self.computeH(thrust) 89 | innov_var = self.computeInnovVar(H) 90 | K = self.computeKalmanGain(H, innov_var) 91 | innov = self.computeInnov(acc_z, thrust) 92 | innov_test_ratio = self.computeInnovTestRatio(innov, innov_var) 93 | 94 | residual = innov 95 | 96 | if self.isTestRatioPassing(innov_test_ratio): 97 | self.updateState(K, innov) 98 | self.updateStateCovariance(K, H) 99 | 100 | residual = self.computeInnov(acc_z, thrust) 101 | 102 | elif not abs(self._innov_test_ratio_signed_lpf) < 0.2: 103 | self.bumpStateVariance() 104 | 105 | self.updateLpf(residual, sign(innov)*innov_test_ratio) 106 | self.updateMeasurementNoise(residual, H) 107 | return (innov, innov_var, innov_test_ratio) 108 | 109 | 110 | def computeH(self, thrust): 111 | return -9.81 * thrust / (self._hover_thr**2) 112 | 113 | def computeInnovVar(self, H): 114 | innov_var = H * self._P * H + (self._R * self._R_gain) 115 | return max(innov_var, self._R) 116 | 117 | def computeKalmanGain(self, H, innov_var): 118 | return self._P * H / innov_var 119 | 120 | def computeInnov(self, acc_z, thrust): 121 | return acc_z - self.predictedAccZ(thrust) 122 | 123 | def predictedAccZ(self, thrust): 124 | return 9.81 * thrust / self._hover_thr - 9.81 125 | 126 | def computeInnovTestRatio(self, innov, innov_var): 127 | return innov**2 / (self._innov_gate_size**2 * innov_var) 128 | 129 | def isTestRatioPassing(self, innov_test_ratio): 130 | return (innov_test_ratio < 1.0) 131 | 132 | def updateState(self, K, innov): 133 | self._hover_thr += K * innov 134 | self._hover_thr = clip(self._hover_thr, 0.1, 0.9) 135 | 136 | def updateStateCovariance(self, K, H): 137 | self._P = clip((1.0 - K * H) * self._P, 1e-10, 1.0) 138 | 139 | def updateMeasurementNoise(self, residual, H): 140 | alpha = self._dt / (self._noise_learning_time_constant + self._dt) 141 | res_no_bias = residual - self._residual_lpf 142 | self._R = clip(self._R * (1.0 - alpha) + alpha * (res_no_bias**2 + H * self._P * H), 1.0, 400.0) 143 | 144 | def bumpStateVariance(self): 145 | self._P += 1000.0 * self._Q * self._dt 146 | 147 | def resetAccelNoise(self): 148 | self._R = 5.0 149 | 150 | def updateLpf(self, residual, innov_test_ratio_signed): 151 | alpha = self._dt / (self._lpf_time_constant + self._dt) 152 | self._residual_lpf = (1.0 - alpha) * self._residual_lpf + alpha * residual 153 | self._innov_test_ratio_signed_lpf = (1.0 - alpha) * self._innov_test_ratio_signed_lpf + alpha * clip(innov_test_ratio_signed, -1.0, 1.0) 154 | 155 | 156 | if __name__ == '__main__': 157 | hover_thr_0 = 0.5 158 | hover_ekf = HoverThrEstimator(hover_thr_0) 159 | assert hover_ekf._hover_thr == hover_thr_0 160 | 161 | hover_thr_noise_0 = 0.2 162 | hover_ekf.setStateVar(hover_thr_noise_0**2) 163 | 164 | assert hover_ekf._P == hover_thr_noise_0**2 165 | 166 | hover_thr_process_noise = 0.01 167 | hover_ekf.setProcessVar(hover_thr_process_noise**2) 168 | assert hover_ekf._Q == hover_thr_process_noise**2 169 | 170 | dt = 0.01 171 | hover_ekf.predict(dt) 172 | assert hover_ekf._hover_thr == hover_thr_0 173 | assert hover_ekf._P == hover_thr_noise_0**2 + hover_thr_process_noise**2 * dt 174 | 175 | accel_noise = 0.1 176 | hover_ekf.setMeasVar(accel_noise**2) 177 | assert hover_ekf._R == accel_noise**2 178 | 179 | hover_ekf.fuseAccZ(0.0, hover_thr_0) 180 | assert hover_ekf._hover_thr == hover_thr_0 181 | -------------------------------------------------------------------------------- /hover_thrust_estimator/hover_thrust_replay.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | File: hover_thrust_replay.py 6 | Author: Mathieu Bresciani 7 | Email: brescianimathieu@gmail.com 8 | Github: https://github.com/bresch 9 | Description: 10 | """ 11 | 12 | from numpy import * 13 | import matplotlib.pylab as plt 14 | from HoverThrEstimator import HoverThrEstimator 15 | from pyulog import ULog 16 | import os 17 | 18 | 19 | def get_data(log, topic_name, variable_name): 20 | for elem in log.data_list: 21 | if elem.name == topic_name: 22 | variable_data = elem.data[variable_name] 23 | break 24 | 25 | return variable_data 26 | 27 | def ms2s_list(time_ms_list): 28 | return [i*1e-6 for i in time_ms_list] 29 | 30 | def run(log_name, showplots): 31 | log = ULog(log_name) 32 | 33 | # Select msgs and copy into arrays 34 | thrust = -get_data(log, 'vehicle_local_position_setpoint', 'thrust[2]') 35 | az = -get_data(log, 'vehicle_local_position', 'az') 36 | vx = get_data(log, 'vehicle_local_position', 'vx') 37 | vy = get_data(log, 'vehicle_local_position', 'vy') 38 | vz = -get_data(log, 'vehicle_local_position', 'vz') 39 | dist_bottom = get_data(log, 'vehicle_local_position', 'dist_bottom') 40 | t = ms2s_list(get_data(log, 'vehicle_local_position_setpoint', 'timestamp')) 41 | t_ekf = ms2s_list(get_data(log, 'vehicle_local_position', 'timestamp')) 42 | 43 | # Downsample ekf estimate to setpoint sample rate 44 | accel = array(interp(t, t_ekf, az)) 45 | vel_x = array(interp(t, t_ekf, vx)) 46 | vel_y = array(interp(t, t_ekf, vy)) 47 | vel_z = array(interp(t, t_ekf, vz)) 48 | 49 | # Estimator initial conditions 50 | hover_thrust_0 = 0.5 51 | hover_thrust_noise_0 = 0.1 52 | P0 = hover_thrust_noise_0**2 53 | hover_thrust_process_noise = 0.0036 # hover thrust change / s 54 | Q = hover_thrust_process_noise**2 55 | accel_noise_0 = sqrt(5.0) 56 | R = accel_noise_0**2 # Rk = R 57 | 58 | # Speed sensitivity reduction 59 | vz_thr = 2.0 60 | vxy_thr = 10.0 61 | 62 | hover_ekf = HoverThrEstimator(hover_thrust_0) 63 | hover_ekf.setStateVar(P0) 64 | hover_ekf.setProcessVar(Q) 65 | hover_ekf.setMeasVar(R) 66 | 67 | # Initialize arrays 68 | n = len(t) 69 | accel_true = zeros(n) 70 | hover_thrust = zeros(n) 71 | hover_thrust_std = zeros(n) 72 | hover_thrust_true = zeros(n) 73 | accel_noise_std = zeros(n) 74 | innov = zeros(n) 75 | innov_std = zeros(n) 76 | innov_test_ratio = zeros(n) 77 | innov_test_ratio_signed_lpf = zeros(n) 78 | residual_lpf = zeros(n) 79 | 80 | for k in range(1, n): 81 | meas_noise_coeff_z = max((abs(vel_z[k]) - vz_thr) + 1.0, 1.0) 82 | meas_noise_coeff_xy = max((sqrt(vel_x[k]**2 + vel_y[k]**2) - vxy_thr) + 1.0, 1.0) 83 | hover_ekf.setMeasVarCoeff(max(meas_noise_coeff_xy**2, meas_noise_coeff_z**2)) 84 | 85 | # Save data 86 | hover_thrust[k] = hover_ekf._hover_thr 87 | hover_thrust_std[k] = sqrt(hover_ekf._P) 88 | dt = t[k] - t[k-1] 89 | 90 | if dist_bottom[k] > 1.0: 91 | # Update the EKF 92 | hover_ekf.predict(dt) 93 | (innov[k], innov_var, innov_test_ratio[k]) = hover_ekf.fuseAccZ(accel[k], thrust[k]) 94 | 95 | innov_std[k] = sqrt(innov_var) 96 | accel_noise_std[k] = sqrt(hover_ekf._R) 97 | innov_test_ratio_signed_lpf[k] = hover_ekf._innov_test_ratio_signed_lpf 98 | residual_lpf[k] = hover_ekf._residual_lpf 99 | 100 | if showplots: 101 | head_tail = os.path.split(log_name) 102 | plotData(t, thrust, accel, accel_noise_std, hover_thrust, hover_thrust_std, innov_test_ratio, innov_test_ratio_signed_lpf, innov, innov_std, residual_lpf, head_tail[1]) 103 | 104 | def plotData(t, thrust, accel, accel_noise_std, hover_thrust, hover_thrust_std, innov_test_ratio, innov_test_ratio_signed_lpf, innov, innov_std, residual_lpf, log_name): 105 | n_plots = 5 106 | ax1 = plt.subplot(n_plots, 1, 1) 107 | ax1.plot(t, thrust * 10.0, '.') 108 | ax1.plot(t, accel, '.') 109 | ax1.plot(t, 3*accel_noise_std, 'g--') 110 | ax1.plot(t, -3*accel_noise_std, 'g--') 111 | ax1.legend(["Thrust (10x)", "AccZ", "Acc noise est"]) 112 | plt.title(log_name) 113 | ax1.grid() 114 | 115 | ax2 = plt.subplot(n_plots, 1, 2, sharex=ax1) 116 | ax2.plot(t, hover_thrust, 'b') 117 | ax2.plot(t, hover_thrust + hover_thrust_std, 'g--') 118 | ax2.plot(t, hover_thrust - hover_thrust_std, 'g--') 119 | ax2.legend(["Ht_Est"]) 120 | 121 | ax3 = plt.subplot(n_plots, 1, 3, sharex=ax1) 122 | ax3.plot(t, hover_thrust_std) 123 | ax3.legend(["Ht_noise"]) 124 | 125 | ax4 = plt.subplot(n_plots, 1, 4, sharex=ax1) 126 | ax4.plot(t, innov_test_ratio) 127 | ax4.plot(t, innov_test_ratio_signed_lpf) 128 | ax4.plot(t, residual_lpf) 129 | ax4.legend(["Test ratio", "Test ratio lpf", "Residual lpf"]) 130 | 131 | ax5 = plt.subplot(n_plots, 1, 5, sharex=ax1) 132 | ax5.plot(t, innov) 133 | ax5.plot(t, innov + innov_std, 'g--') 134 | ax5.plot(t, innov - innov_std, 'g--') 135 | ax5.legend(["Innov"]) 136 | ax5.grid() 137 | 138 | plt.show() 139 | 140 | if __name__ == '__main__': 141 | import argparse 142 | 143 | # Get the path of this script (without file name) 144 | script_path = os.path.split(os.path.realpath(__file__))[0] 145 | 146 | # Parse arguments 147 | parser = argparse.ArgumentParser( 148 | description='Estimate mag biases from ULog file') 149 | 150 | # Provide parameter file path and name 151 | parser.add_argument('logfile', help='Full ulog file path, name and extension', type=str) 152 | parser.add_argument('--showplots', help='Display relevant plots', 153 | action='store_true') 154 | args = parser.parse_args() 155 | 156 | logfile = os.path.abspath(args.logfile) # Convert to absolute path 157 | 158 | run(logfile, args.showplots) 159 | -------------------------------------------------------------------------------- /hover_thrust_estimator/hover_thrust_sim.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | File: hover_thrust_sim.py 6 | Author: Mathieu Bresciani 7 | Email: brescianimathieu@gmail.com 8 | Github: https://github.com/bresch 9 | Description: 10 | Simple simulation of for the HoverThrEstimator class. 11 | A thrust curve is generated and produces acceleration 12 | using the same model as the estimator but with noise: 13 | 14 | accel = (Thrust - Hover thrust) * mass coefficient + noise 15 | where the mass coefficient is g / Hover thrust 16 | which gives 17 | accel = g * Thrust / Hover thrust - g 18 | """ 19 | 20 | from numpy import * 21 | import matplotlib.pylab as plt 22 | from HoverThrEstimator import HoverThrEstimator 23 | 24 | def getAccelFromThrTime(thrust, t, ht): 25 | accel = 9.81 * thrust / ht - 9.81 26 | 27 | return accel 28 | 29 | def getThrFromTime(t): 30 | if t <= 10.0: 31 | thrust = 0.8 32 | else: 33 | thrust = 0.5 34 | 35 | return thrust 36 | 37 | def getHoverThrustFromTime(t): 38 | if t <= 10.0: 39 | hover_t = 0.8 40 | else: 41 | hover_t = 0.5 42 | 43 | return hover_t 44 | 45 | 46 | if __name__ == '__main__': 47 | # Simulation parameters 48 | dt = 0.03 49 | t_end = 60.0 50 | t = arange (0.0, t_end+dt, dt) 51 | n = len(t) 52 | 53 | 54 | # Estimator initial conditions 55 | hover_thrust_0 = 0.5 56 | hover_thrust_noise_0 = 0.1 57 | P0 = hover_thrust_noise_0**2 58 | hover_thrust_process_noise = sqrt(0.25e-6) # hover thrust change / s 59 | Q = hover_thrust_process_noise**2 60 | Qk = Q 61 | accel_noise_0 = sqrt(5.0) 62 | R = accel_noise_0**2 # Rk = R 63 | 64 | hover_ekf = HoverThrEstimator(hover_thrust_0) 65 | hover_ekf.setStateVar(P0) 66 | hover_ekf.setProcessVar(Qk) 67 | hover_ekf.setMeasVar(R) 68 | 69 | accel_noise_sim = 2.0 # in m/s2 70 | hover_thrust_sim = 0.8 71 | 72 | # Create data buckets 73 | accel = zeros(n) 74 | accel_true = zeros(n) 75 | thrust = ones(n) 76 | hover_thrust = zeros(n) 77 | hover_thrust_std = zeros(n) 78 | hover_thrust_true = zeros(n) 79 | accel_noise_std = zeros(n) 80 | innov_test_ratio_signed_lpf = zeros(n) 81 | innov = zeros(n) 82 | innov_std = zeros(n) 83 | innov_test_ratio = zeros(n) 84 | residual_lpf = zeros(n) 85 | 86 | for k in range(0, n): 87 | # Save data 88 | hover_thrust[k] = hover_ekf._hover_thr 89 | hover_thrust_std[k] = sqrt(hover_ekf._P) 90 | 91 | # Generate measurement 92 | thrust[k] = getThrFromTime(t[k]) 93 | noise = random.randn() * accel_noise_sim 94 | hover_thrust_true[k] = getHoverThrustFromTime(t[k]) 95 | accel_true[k] = getAccelFromThrTime(thrust[k], t[k], hover_thrust_true[k]) 96 | accel[k] = accel_true[k] + noise 97 | 98 | # Accel noise change 99 | if t[k] > 40.0: 100 | accel_noise_sim = 2.0 101 | elif t[k] > 30.0: 102 | accel_noise_sim = 4.0 103 | 104 | # Update the EKF 105 | hover_ekf.predict(dt) 106 | (innov[k], innov_var, innov_test_ratio[k]) = hover_ekf.fuseAccZ(accel[k], thrust[k]) 107 | innov_std[k] = sqrt(innov_var) 108 | accel_noise_std[k] = sqrt(hover_ekf._R) 109 | innov_test_ratio_signed_lpf[k] = hover_ekf._innov_test_ratio_signed_lpf 110 | residual_lpf[k] = hover_ekf._residual_lpf 111 | # print("P = ", hover_ekf._P, "\tQ = ", hover_ekf._Q, "\tsqrt(R) = ", sqrt(hover_ekf._R)) 112 | 113 | # Plot results 114 | n_plots = 5 115 | ax1 = plt.subplot(n_plots, 1, 1) 116 | ax1.plot(t, thrust * 10.0, '.') 117 | ax1.plot(t, accel, '.') 118 | ax1.plot(t, accel_true + 3*accel_noise_std, 'g--') 119 | ax1.plot(t, accel_true - 3*accel_noise_std, 'g--') 120 | ax1.legend(["Thrust (10x)", "AccZ"]) 121 | 122 | ax2 = plt.subplot(n_plots, 1, 2, sharex=ax1) 123 | ax2.plot(t, hover_thrust, 'b') 124 | ax2.plot(t, hover_thrust_true, 'k:') 125 | ax2.plot(t, hover_thrust + hover_thrust_std, 'g--') 126 | ax2.plot(t, hover_thrust - hover_thrust_std, 'g--') 127 | ax2.legend(["Ht_Est", "Ht_True"]) 128 | 129 | ax3 = plt.subplot(n_plots, 1, 3, sharex=ax1) 130 | ax3.plot(t, hover_thrust_std) 131 | ax3.legend(["Ht_noise"]) 132 | 133 | ax4 = plt.subplot(n_plots, 1, 4, sharex=ax1) 134 | ax4.plot(t, innov_test_ratio) 135 | ax4.plot(t, innov_test_ratio_signed_lpf) 136 | ax4.plot(t, residual_lpf) 137 | ax4.legend(["Test ratio", "Test ratio lpf", "Residual lpf"]) 138 | 139 | ax5 = plt.subplot(n_plots, 1, 5, sharex=ax1) 140 | ax5.plot(t, innov) 141 | ax5.legend(["Innov"]) 142 | ax5.grid() 143 | 144 | plt.show() 145 | -------------------------------------------------------------------------------- /leaky_integrator/README.md: -------------------------------------------------------------------------------- 1 | # Leaky integrator 2 | Prototyping script for the simple single pole IIR filter of the form y[n] = alpha * y[n-1] + (alpha - 1) * x[n] well known as leaky integrator. 3 | 4 | ## Setup 5 | To run the script you need Python 3 (tested on 3.8.2) and the following python modules: 6 | ``` 7 | sudo python3 -m pip install numpy matplotlib 8 | ``` 9 | 10 | Command to run the script: 11 | ``` 12 | python3 leaky_integrator.py 13 | ``` 14 | 15 | ## Details 16 | 17 | Useful practical reads: 18 | - https://en.wikipedia.org/wiki/Leaky_integrator 19 | - https://dsp.stackexchange.com/questions/43851/is-there-a-common-name-for-the-first-order-iir-averaging-filter 20 | - https://dsp.stackexchange.com/questions/3179/is-a-leaky-integrator-the-same-thing-as-a-low-pass-filter 21 | 22 | The visualization shows plots of a signal (orange) the noisy measured signal (blue) and the leaky integrator filtered signal (green) with 1e2, 1e3, 1e4, 1e5 samples over 10s. The goal is to calculate the filter parameter alpha such that the time constant stays the same independent of the filter frequency. alpha = tau / (tau + dt) with tau being the time constant and dt the time between two samples. 23 | -------------------------------------------------------------------------------- /leaky_integrator/leaky_integrator.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """ 4 | File: leaky_integrator.py 5 | Author: Matthias Grob 6 | Email: maetugr@gmail.com 7 | Github: https://github.com/maetugr 8 | Description: 9 | Prototyping script for the simple single pole IIR filter of the form 10 | y[n] = alpha * y[n-1] + (alpha - 1) * x[n] 11 | well known as leaky integrator. 12 | Requires numpy, matplotlib 13 | """ 14 | 15 | import numpy as np 16 | import matplotlib.pylab as plt 17 | 18 | def runExperiment(total_time, samples, axis): 19 | # time setup 20 | dt = total_time / samples 21 | t = np.arange(0, total_time, dt) 22 | 23 | # filter configuration 24 | tau = 0.5 25 | alpha = tau / (tau + dt) 26 | 27 | # signal and noise setup 28 | np.random.seed(1) 29 | signal = 1 + (t / total_time) 30 | noise = np.random.rand(samples) - 0.5 31 | noisy = signal + noise 32 | 33 | # filter 34 | filtered = np.zeros(samples) 35 | filtered[0] = 0.0 36 | for n in range(1, samples): 37 | filtered[n] = alpha * filtered[n-1] + (1 - alpha) * noisy[n] 38 | 39 | axis.plot(t, noisy, label = "noisy") 40 | axis.plot(t, signal, label = "signal") 41 | axis.plot(t, filtered, label = "filtered") 42 | 43 | 44 | # plotting 45 | fig, axs = plt.subplots(4, 1) 46 | 47 | runExperiment(10, 100, axs[0]) 48 | runExperiment(10, 1000, axs[1]) 49 | runExperiment(10, 10000, axs[2]) 50 | runExperiment(10, 100000, axs[3]) 51 | 52 | plt.show() 53 | -------------------------------------------------------------------------------- /quaternion_attitude_control/README.md: -------------------------------------------------------------------------------- 1 | # Quaternion attitude control 2 | The algorithm implements an attitude control strategy for flying vehicles especially multicopters using unit quaternion rotation math. 3 | 4 | ## Setup 5 | To run the script you need Python 3 (tested on 3.7.1) and the following python modules: 6 | ``` 7 | sudo python3 -m pip install numpy matplotlib pyquaternion 8 | ``` 9 | 10 | Command to run the script: 11 | ``` 12 | python3 quaternion_attitude_control_test.py 13 | ``` 14 | 15 | ## Details 16 | The algorithm is based on [this paper](https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf) and [this master thesis](https://drive.google.com/uc?e=pdf&id=1jVABlvL4eGU_IM6f_tUnRhjHgKOIAlcP). 17 | 18 | The visualization shows an abstract vehicle state for each time step in the form of three arrows. The common origin of the arrows is the vehicle's 3D position at the timestep and the arrows point in x-, y- and z-axis (red, gree, blue) direction of the vehicle's current body frame and hence represent its attitude relative to the world frame. 19 | -------------------------------------------------------------------------------- /quaternion_attitude_control/quaternion_attitude_control_test.py: -------------------------------------------------------------------------------- 1 | """ 2 | Copyright (c) 2018 PX4 Development Team 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions 6 | are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in 12 | the documentation and/or other materials provided with the 13 | distribution. 14 | 3. Neither the name PX4 nor the names of its contributors may be 15 | used to endorse or promote products derived from this software 16 | without specific prior written permission. 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 20 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 21 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 22 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 23 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 24 | OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 25 | AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 26 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 27 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | POSSIBILITY OF SUCH DAMAGE. 29 | 30 | File: quaternion_attitude_control_test.py 31 | Author: Matthias Grob https://github.com/MaEtUgR 32 | License: BSD 3-Clause 33 | Description: 34 | This file runs a minimal, ideal multicopter test simulation 35 | to verify attitude control strategies and visualize the result. 36 | """ 37 | 38 | import numpy as np 39 | from numpy import linalg as la 40 | 41 | from pyquaternion import Quaternion 42 | from math import sin, cos, asin, acos, degrees, radians, sqrt 43 | 44 | import matplotlib.pyplot as plt 45 | from mpl_toolkits.mplot3d import Axes3D 46 | 47 | def plotrotc(q = Quaternion(), p = (0,0,0)): 48 | """ 49 | Plot 3D RGB (x,y,z) axes of the vehicle body frame body frame 50 | 51 | Params: 52 | q: [optional] body attitude quaternion. Defaults to level. 53 | p: [optional] 3D body position in space Defaults to origin. 54 | """ 55 | # convert the quaternion to a rotation metrix because the columns are the base vectors 56 | R = q.rotation_matrix 57 | # plot unit vectors from the body position into the 3 base vector directions 58 | ay.quiver(*p, *R[:,0], color='red') 59 | ay.quiver(*p, *R[:,1], color='green') 60 | ay.quiver(*p, *R[:,2], color='blue') 61 | 62 | def qcontrol_full(q = Quaternion(), qd = Quaternion()): 63 | """ 64 | Calculate angular velocity to get from current to desired attitude 65 | All axes are treated equally hence the name "full". 66 | 67 | Params: 68 | q: [optional] current body attitude quaternion. Defaults to level. 69 | qd: [optional] desired body attitude quaternion setpoint. Defaults to level. 70 | 71 | Returns: 72 | angular velocity to apply to the body to reach the specified setpoint 73 | """ 74 | # quaternion attitude control law, qe is rotation from q to qd 75 | qe = q.inverse * qd 76 | # using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle) 77 | # also taking care of the antipodal unit quaternion ambiguity 78 | return 2 * np.sign(qe[0]+1e-10) * np.array([qe[1], qe[2], qe[3]]) 79 | 80 | def qcontrol_reduced(q = Quaternion(), qd = Quaternion(), yw = 1): 81 | """ 82 | Calculate angular velocity to get from current to desired attitude 83 | The body yaw axis has less priority then roll and pitch hence the name "reduced". 84 | 85 | Params: 86 | q: [optional] current body attitude quaternion. Defaults to level. 87 | qd: [optional] desired body attitude quaternion setpoint. Defaults to level. 88 | yw: [optional] desired body attitude quaternion setpoint. Defaults to full yaw priority. 89 | 90 | Returns: 91 | angular velocity to apply to the body to reach the specified setpoint 92 | """ 93 | # extract body z-axis and desired body z-axis 94 | ez = dcm_z(q) 95 | ezd = dcm_z(qd) 96 | # reduced rotation that only aligns the body z-axis 97 | qd_red = vtoq(ez, ezd) 98 | # transform rotation from current to desired z-axis 99 | # into a world frame reduced desired attitude 100 | qd_red *= q 101 | 102 | # mix full and reduced desired attitude using yaw weight 103 | q_mix = qd_red.inverse * qd 104 | q_mix *= np.sign(q_mix[0]) 105 | # catch numerical problems with the domain of acosf and asinf 106 | q_mix.q = np.clip(q_mix.q, -1, 1) 107 | qd = qd_red * Quaternion(cos(yw * acos(q_mix[0])), 0, 0, sin(yw * asin(q_mix[3]))) 108 | 109 | # quaternion attitude control law, qe is rotation from q to qd 110 | qe = q.inverse * qd 111 | # using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle) 112 | # also taking care of the antipodal unit quaternion ambiguity 113 | return 2 * np.sign(qe[0]+1e-10) * np.array([qe[1], qe[2], qe[3]]) 114 | 115 | def ftoq(f = np.array([0,0,1]), yaw = 0): 116 | """ 117 | Calculate a desired attitude from 3D thrust and yaw in world frame 118 | Assuming a default multicopter configuration. 119 | 120 | Note: 0 yaw in world frame results in body x-axis projection 121 | onto world x-y-plane being aligned with world x-axis. 122 | 123 | Params: 124 | f: [optional] desired 3D thrust vector in world frame. Defaults to upwards. 125 | yaw: [optional] desired body yaw. 126 | 127 | Returns: 128 | attitude quaternion setpoint which meets the given goals 129 | """ 130 | # align z-axis with thrust 131 | body_z = f / la.norm(f) 132 | # choose body x-axis orthogonal to z and with given yaw 133 | yaw_direction = np.array([-sin(yaw), cos(yaw), 0]) 134 | body_x = np.cross(yaw_direction, body_z) 135 | body_x /= la.norm(body_x) 136 | # choose body y-axis right hand orthogonal to x- and z-axis 137 | body_y = np.cross(body_z, body_x) 138 | # assemble attitude matrix and quaternion from base vectors 139 | Rd = np.column_stack((body_x, body_y, body_z)) 140 | return Quaternion(matrix=Rd) 141 | 142 | def dcm_z(q = Quaternion()): 143 | """ 144 | Calculate the body z-axis base vector from body attitude. 145 | 146 | Params: 147 | q: [optional] body attitude quaternion. Defaults to level. 148 | 149 | Returns: 150 | unit base vector of body z-axis in world frame 151 | """ 152 | # convert quaternion to rotation matrix 153 | R = q.rotation_matrix 154 | # take last column vector 155 | return R[:,2] 156 | 157 | def vtoq(src = np.array([0,0,1]), dst = np.array([0,0,1]), eps = 1e-5): 158 | """ 159 | Calculate quaternion representing the shortest rotation 160 | from one vector to the other 161 | 162 | Params: 163 | src: [optional] Source 3D vector from which to start the rotation. Defaults to upwards direction. 164 | dst: [optional] Destination 3D vector to which to rotate. Defaults to upwards direction. 165 | eps: [optional] numerical thershold to catch 180 degree rotations. Defaults to 1e-5. 166 | 167 | Returns: 168 | quaternion rotationg the shortest way from src to dst 169 | """ 170 | q = Quaternion() 171 | cr = np.cross(src, dst) 172 | dt = np.dot(src, dst) 173 | 174 | if(la.norm(cr) < eps and dt < 0): 175 | # handle corner cases with 180 degree rotations 176 | # if the two vectors are parallel, cross product is zero 177 | # if they point opposite, the dot product is negative 178 | cr = np.abs(src) 179 | if(cr[0] < cr[2]): 180 | if(cr[0] < cr[2]): 181 | cr = np.array([1,0,0]) 182 | else: 183 | cr = np.array([0,0,1]) 184 | else: 185 | if(cr[1] < cr[2]): 186 | cr = np.array([0,1,0]) 187 | else: 188 | cr = np.array([0,0,1]) 189 | q[0] = 0 190 | else: 191 | # normal case, do half-way quaternion solution 192 | q[0] = dt + sqrt(np.dot(src,src) * np.dot(dst,dst)) 193 | q[1] = cr[0] 194 | q[2] = cr[1] 195 | q[3] = cr[2] 196 | return q.normalised 197 | 198 | # setup 3D plot 199 | fig = plt.figure('Quaternion attitude control') 200 | ay = fig.add_subplot(111, projection='3d') 201 | ay.set_xlim([-5, 1]) 202 | ay.set_ylim([-5, 1]) 203 | ay.set_zlim([-5, 1]) 204 | ay.set_xlabel('X') 205 | ay.set_ylabel('Y') 206 | ay.set_zlabel('Z') 207 | 208 | # initialize state 209 | steps = 0 210 | q = qd = Quaternion() # atttitude state 211 | v = np.array([0.,0.,0.]) # velocity state 212 | p = np.array([0.,0.,0.]) # position state 213 | 214 | # specify goal and parameters 215 | pd = np.array([-5,0,0]) # desired position 216 | yd = radians(180) # desired yaw 217 | dt = 0.2 # time steps 218 | 219 | # run simulation until the goal is reached 220 | while steps < 1000 and (not np.isclose(p, pd).all() or (q.inverse * qd).degrees > 0.1): 221 | # plot current vehicle body state abstraction 222 | plotrotc(q, p) 223 | 224 | # run minimal position & velocity control 225 | vd = (pd - p) 226 | fd = (vd - v) 227 | fd += np.array([0,0,1]) # "gravity" 228 | 229 | # run attitude control 230 | qd = ftoq(fd, yd) 231 | thrust = np.dot(fd, dcm_z(q)) 232 | w = 3*qcontrol_reduced(q, qd, 0.4) 233 | 234 | # propagate states with minimal, ideal simulation 235 | q.integrate(w, dt) 236 | f = dcm_z(q) * thrust 237 | v += f - np.array([0,0,1]) 238 | p += dt*v 239 | 240 | # print progress 241 | steps += 1 242 | print(steps, '\t', p) 243 | 244 | plt.show() -------------------------------------------------------------------------------- /range_finder_kinematic_consistency/AlphaFilter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Copyright (c) 2022 PX4 Development Team 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions 7 | are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in 13 | the documentation and/or other materials provided with the 14 | distribution. 15 | 3. Neither the name PX4 nor the names of its contributors may be 16 | used to endorse or promote products derived from this software 17 | without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | 32 | File: AlphaFilter.py 33 | Author: Mathieu Bresciani 34 | License: BSD 3-Clause 35 | Description: 36 | """ 37 | 38 | import sys 39 | import numpy as np 40 | 41 | M_PI_F = 3.14159265 42 | FLT_EPSILON = sys.float_info.epsilon 43 | 44 | class AlphaFilter(object): 45 | 46 | def __init__(self): 47 | self._cutoff_freq = 0.0 48 | self._alpha = 0.0 49 | self._filter_state = 0.0 50 | 51 | # Set filter parameters for time abstraction 52 | # 53 | # Both parameters have to be provided in the same units. 54 | # 55 | # @param sample_interval interval between two samples 56 | # @param time_constant filter time constant determining convergence 57 | def setParameters(self, sample_interval, time_constant): 58 | denominator = time_constant + sample_interval 59 | 60 | if denominator > FLT_EPSILON: 61 | self.setAlpha(sample_interval / denominator) 62 | 63 | def setCutoffFreq(self, sample_freq, cutoff_freq): 64 | if (sample_freq <= 0.0) or (cutoff_freq <= 0.0) or (cutoff_freq >= sample_freq / 2.0) or not np.isfinite(sample_freq) or not np.isfinite(cutoff_freq): 65 | # Invalid parameters 66 | return False 67 | 68 | self.setParameters(1.0 / sample_freq, 1.0 / (2.0 * M_PI_F * cutoff_freq)) 69 | self._cutoff_freq = cutoff_freq 70 | return True 71 | 72 | # Set filter parameter alpha directly without time abstraction 73 | # 74 | # @param alpha [0,1] filter weight for the previous state. High value - long time constant. 75 | def setAlpha(self, alpha): 76 | self._alpha = alpha 77 | 78 | # Set filter state to an initial value 79 | # 80 | # @param sample new initial value 81 | def reset(self, sample): 82 | self._filter_state = sample 83 | 84 | # Add a new raw value to the filter 85 | # 86 | # @return retrieve the filtered result 87 | def update(self, sample): 88 | self._filter_state = self.updateCalculation(sample) 89 | return self._filter_state 90 | 91 | def getState(self): 92 | return self._filter_state 93 | 94 | def getCutoffFreq(self): 95 | return self._cutoff_freq 96 | 97 | def updateCalculation(self, sample): 98 | return (1.0 - self._alpha) * self._filter_state + self._alpha * sample 99 | 100 | if __name__ == '__main__': 101 | lpf = AlphaFilter() 102 | lpf.setAlpha(0.1) 103 | 104 | for i in range(1, 100): 105 | out = lpf.update(1) 106 | print(out); 107 | 108 | -------------------------------------------------------------------------------- /range_finder_kinematic_consistency/RangeFinderConsistencyCheck.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Copyright (c) 2022 PX4 Development Team 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions 7 | are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in 13 | the documentation and/or other materials provided with the 14 | distribution. 15 | 3. Neither the name PX4 nor the names of its contributors may be 16 | used to endorse or promote products derived from this software 17 | without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | 32 | File: ConsistencyEstimator.py 33 | Author: Mathieu Bresciani 34 | Email: brescianimathieu@gmail.com 35 | Github: https://github.com/bresch 36 | Description: 37 | """ 38 | 39 | import numpy as np 40 | import sys 41 | import math 42 | import matplotlib.pylab as plt 43 | from AlphaFilter import AlphaFilter 44 | 45 | FLT_EPSILON = sys.float_info.epsilon 46 | NAN = float('nan') 47 | verbose = True; 48 | 49 | if verbose: 50 | def verboseprint(*args): 51 | # Print each argument separately so caller doesn't need to 52 | # stuff everything to be printed into a single string 53 | for arg in args: 54 | print(arg) 55 | print() 56 | else: 57 | verboseprint = lambda *a: None # do-nothing function 58 | 59 | class RangeFinderConsistencyCheck(object): 60 | def __init__(self): 61 | self._time_last_update_us = 0 62 | self._dist_bottom_prev = 0.0 63 | self._time_last_inconsistent_us = 0 64 | self._vel_bottom_gate = 0.1 65 | self._vel_bottom_signed_test_ratio_tau = 2.0 66 | self._consistency_hyst_time_us = 1.0 67 | self._min_vz_for_valid_consistency = 0.5 68 | self._vel_bottom_signed_test_ratio_lpf = AlphaFilter() 69 | self._is_kinematically_consistent = True 70 | 71 | self._vel_bottom_innov = 0.0 72 | self._vel_bottom_innov_var = 0.0 73 | self._vel_bottom_test_ratio = 0.0 74 | 75 | def update(self, dist_bottom, dist_bottom_var, vz, vz_var, time_us): 76 | dt = (time_us - self._time_last_update_us) * 1e-6 77 | 78 | if (self._time_last_update_us == 0) or (dt < 0.001) or (dt > 0.5): 79 | self._time_last_update_us = time_us 80 | self._dist_bottom_prev = dist_bottom 81 | return; 82 | 83 | vel_bottom = (dist_bottom - self._dist_bottom_prev) / dt 84 | innov = -vel_bottom - vz; # vel_bottom is +up while vz is +down 85 | vel_bottom_var = 2.0 * dist_bottom_var / (dt * dt) # Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2 86 | innov_var = vel_bottom_var + vz_var 87 | normalized_innov_sq = (innov * innov) / innov_var 88 | self._vel_bottom_test_ratio = normalized_innov_sq / (self._vel_bottom_gate * self._vel_bottom_gate) 89 | 90 | self._vel_bottom_signed_test_ratio_lpf.setParameters(dt, self._vel_bottom_signed_test_ratio_tau) 91 | signed_test_ratio = np.sign(innov) * self._vel_bottom_test_ratio 92 | self._vel_bottom_signed_test_ratio_lpf.update(signed_test_ratio) 93 | 94 | self.updateConsistency(vz, time_us) 95 | 96 | self._time_last_update_us = time_us 97 | self._dist_bottom_prev = dist_bottom 98 | 99 | # Save for logging 100 | self._vel_bottom_innov = innov 101 | self._vel_bottom_innov_var = innov_var 102 | 103 | def updateConsistency(self, vz, time_us): 104 | if abs(self._vel_bottom_signed_test_ratio_lpf.getState()) >= 1.0: 105 | self._is_kinematically_consistent = False 106 | self._time_last_inconsistent_us = time_us 107 | 108 | else: 109 | if abs(vz) > self._min_vz_for_valid_consistency and self._vel_bottom_test_ratio < 1.0 and ((time_us - self._time_last_inconsistent_us) > self._consistency_hyst_time_us): 110 | self._is_kinematically_consistent = True 111 | 112 | def getInnov(self): 113 | return self._vel_bottom_innov 114 | 115 | def isKinematicallyConsistent(self): 116 | return self._is_kinematically_consistent 117 | 118 | def getSignedTestRatioLpf(self): 119 | return self._vel_bottom_signed_test_ratio_lpf.getState() 120 | 121 | if __name__ == '__main__': 122 | rng_kin = RangeFinderConsistencyCheck() 123 | inn = [] 124 | dt = 0.1 125 | 126 | for i in range(1, 100): 127 | rng_kin.update(i*dt, 0.1, -1.0, 0.001, i*dt * 1e6); 128 | inn.append(rng_kin.getInnov()) 129 | 130 | import matplotlib.pylab as plt 131 | plt.plot(inn) 132 | plt.show() 133 | -------------------------------------------------------------------------------- /range_finder_kinematic_consistency/data_extractor.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Copyright (c) 2022 PX4 Development Team 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions 7 | are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in 13 | the documentation and/or other materials provided with the 14 | distribution. 15 | 3. Neither the name PX4 nor the names of its contributors may be 16 | used to endorse or promote products derived from this software 17 | without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | 32 | File: data_extractor.py 33 | Author: Mathieu Bresciani 34 | License: BSD 3-Clause 35 | Description: 36 | """ 37 | 38 | import numpy as np 39 | from scipy import signal 40 | from pyulog import ULog 41 | 42 | def getAllData(logfile): 43 | log = ULog(logfile) 44 | 45 | rng = getData(log, 'distance_sensor', 'current_distance') 46 | t_rng = ms2s(getData(log, 'distance_sensor', 'timestamp')) 47 | 48 | vz = getData(log, 'vehicle_local_position', 'vz') 49 | t_vz = ms2s(getData(log, 'vehicle_local_position', 'timestamp')) 50 | 51 | STATE_VZ = 6 52 | vz_var = getData(log, 'estimator_states', f'covariances[{STATE_VZ}]') 53 | t_vz_var = ms2s(getData(log, 'estimator_states', 'timestamp')) 54 | 55 | (t_aligned, rng_aligned, vz_aligned, vz_var_aligned) = alignData(log, t_rng, rng, t_vz, vz, t_vz_var, vz_var) 56 | 57 | t_aligned -= t_aligned[0] 58 | 59 | return (t_aligned, rng_aligned, vz_aligned, vz_var_aligned) 60 | 61 | def getData(log, topic_name, variable_name, instance=0): 62 | variable_data = np.array([]) 63 | for elem in log.data_list: 64 | if elem.name == topic_name: 65 | if instance == elem.multi_id: 66 | variable_data = elem.data[variable_name] 67 | break 68 | 69 | return variable_data 70 | 71 | def ms2s(time_ms): 72 | return time_ms * 1e-6 73 | 74 | def getDeltaMean(data_list): 75 | dx = 0 76 | length = len(data_list) 77 | for i in range(1,length): 78 | dx = dx + (data_list[i]-data_list[i-1]) 79 | 80 | dx = dx/(length-1) 81 | return dx 82 | 83 | def alignData(log, t_u_data, u_data, t_y_data, y_data, t_y2_data, y2_data): 84 | len_y = len(t_y_data) 85 | len_y2 = len(t_y2_data) 86 | i_y = 0 87 | i_y2 = 0 88 | u_aligned = [] 89 | y_aligned = [] 90 | y2_aligned = [] 91 | t_aligned = [] 92 | 93 | for i_u in range(len(t_u_data)): 94 | t_u = t_u_data[i_u] 95 | 96 | while t_y_data[i_y] <= t_u and i_y < len_y-1: 97 | i_y += 1 98 | while t_y2_data[i_y2] <= t_u and i_y2 < len_y2-1: 99 | i_y2 += 1 100 | 101 | u_aligned = np.append(u_aligned, u_data[i_u]) 102 | y_aligned = np.append(y_aligned, y_data[i_y-1]) 103 | y2_aligned = np.append(y2_aligned, y2_data[i_y2-1]) 104 | t_aligned.append(t_u) 105 | 106 | return (t_aligned, u_aligned, y_aligned, y2_aligned) 107 | 108 | if __name__ == '__main__': 109 | import argparse 110 | import os 111 | 112 | parser = argparse.ArgumentParser( 113 | description='Extract data from a give .ulg file') 114 | 115 | parser.add_argument('logfile', help='Full ulog file path, name and extension', type=str) 116 | args = parser.parse_args() 117 | 118 | logfile = os.path.abspath(args.logfile) # Convert to absolute path 119 | 120 | (t_aligned, u_aligned, y_aligned, y2_data) = getAllData(logfile) 121 | -------------------------------------------------------------------------------- /range_finder_kinematic_consistency/range_finder_consistency_check_replay.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Copyright (c) 2022 PX4 Development Team 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions 7 | are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | 2. Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in 13 | the documentation and/or other materials provided with the 14 | distribution. 15 | 3. Neither the name PX4 nor the names of its contributors may be 16 | used to endorse or promote products derived from this software 17 | without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | POSSIBILITY OF SUCH DAMAGE. 31 | 32 | File: range_finder_consistency_check.py 33 | Author: Mathieu Bresciani 34 | Email: brescianimathieu@gmail.com 35 | Github: https://github.com/bresch 36 | Description: 37 | """ 38 | 39 | import numpy as np 40 | import matplotlib.pylab as plt 41 | from data_extractor import getAllData 42 | from RangeFinderConsistencyCheck import RangeFinderConsistencyCheck 43 | 44 | def run(logfile): 45 | (t, rng, vz, vz_var) = getAllData(logfile) 46 | 47 | rng_kin = RangeFinderConsistencyCheck() 48 | rng_kin._vel_bottom_gate = 1.0 49 | 50 | n = len(t) 51 | inn = [0.0] 52 | rng_var = [0.0] 53 | test_ratio = [0.0] 54 | signed_test_ratio_lpf = [0.0] 55 | vel_bottom_var = [0.0] 56 | is_consistent = [False] 57 | 58 | for k in range(1, n): 59 | rng_var.append(0.01**2 + (0.01*rng[k])**2) 60 | rng_kin.update(rng[k], rng_var[k], vz[k], vz_var[k], t[k]*1e6) 61 | inn.append(rng_kin.getInnov()) 62 | test_ratio.append(rng_kin._vel_bottom_test_ratio) 63 | signed_test_ratio_lpf.append(rng_kin.getSignedTestRatioLpf()) 64 | is_consistent.append(rng_kin.isKinematicallyConsistent()) 65 | vel_bottom_var.append(rng_kin._vel_bottom_innov_var - rng_var[k]) 66 | 67 | plotData(t, rng, rng_var, is_consistent, vz, inn, test_ratio, signed_test_ratio_lpf, vel_bottom_var) 68 | 69 | def plotData(t, rng, rng_var, is_consistent, vz, inn, test_ratio, signed_test_ratio_lpf, vel_bottom_var): 70 | n_plots = 4 71 | ax1 = plt.subplot(n_plots, 1, 1) 72 | ax1.plot(t, rng) 73 | ax1.plot(t, rng+np.sqrt(rng_var), 'r--') 74 | ax1.plot(t, rng-np.sqrt(rng_var), 'r--') 75 | ax1.legend(["rng"]) 76 | ax1.grid() 77 | 78 | ax2 = plt.subplot(n_plots, 1, 2, sharex=ax1) 79 | ax2.plot(t, test_ratio) 80 | ax2.plot(t, signed_test_ratio_lpf) 81 | ax2.legend(["test_ratio", "signed_test_ratio_lpf"]) 82 | ax2.set_ylim(-5, 5) 83 | ax2.grid() 84 | 85 | ax3 = plt.subplot(n_plots, 1, 3, sharex=ax1) 86 | ax3.plot(t, vz) 87 | rng_dot = vz+inn 88 | ax3.plot(t, rng_dot) 89 | ax3.plot(t, rng_dot+np.sqrt(vel_bottom_var), 'r--') 90 | ax3.plot(t, rng_dot-np.sqrt(vel_bottom_var), 'r--') 91 | ax3.set_ylim(-5, 5) 92 | ax3.legend(["vz", "rng_dot"]) 93 | 94 | ax4 = plt.subplot(n_plots, 1, 4, sharex=ax1) 95 | ax4.plot(t, is_consistent) 96 | ax4.legend(["is_consistent"]) 97 | 98 | plt.show() 99 | 100 | if __name__ == '__main__': 101 | import os 102 | import argparse 103 | 104 | # Get the path of this script (without file name) 105 | script_path = os.path.split(os.path.realpath(__file__))[0] 106 | 107 | # Parse arguments 108 | parser = argparse.ArgumentParser( 109 | description='Estimate mag biases from ULog file') 110 | 111 | # Provide parameter file path and name 112 | parser.add_argument('logfile', help='Full ulog file path, name and extension', type=str) 113 | args = parser.parse_args() 114 | 115 | logfile = os.path.abspath(args.logfile) # Convert to absolute path 116 | 117 | run(logfile) 118 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | matplotlib 2 | numpy 3 | pyquaternion 4 | pyulog 5 | scipy 6 | sympy 7 | -------------------------------------------------------------------------------- /trajectory_generator/README.md: -------------------------------------------------------------------------------- 1 | # trajectory_generator 2 | Output: 3 | 4 | ![](/images/vel_traj_graph.png) 5 | -------------------------------------------------------------------------------- /trajectory_generator/VelocitySmoothing.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | File: VelocitySmoothing.py 6 | Author: Mathieu Bresciani 7 | Email: brescianimathieu@gmail.com 8 | Github: https://github.com/bresch 9 | Description: 10 | Given a desired velocity setpoint v_d, the trajectory generator computes 11 | a time-optimal trajectory satisfaying the following variable constraints: 12 | - j_max : maximum jerk 13 | - a_max : maximum acceleration 14 | - v_max : maximum velocity 15 | - a0 : initial acceleration 16 | - v0 : initial velocity 17 | - v3 : final velocity 18 | The hard constraint used to generate the optimizer is: 19 | - a3 = 0.0 : final acceleration 20 | 21 | The trajectory generated is made by three parts: 22 | 1) Increasing acceleration during T1 seconds 23 | 2) Constant acceleration during T2 seconds 24 | 3) Decreasing acceleration during T3 seconds 25 | """ 26 | 27 | from __future__ import print_function 28 | 29 | from numpy import * 30 | import sys 31 | import math 32 | import matplotlib.pylab as plt 33 | 34 | FLT_EPSILON = sys.float_info.epsilon 35 | NAN = float('nan') 36 | verbose = True 37 | 38 | if verbose: 39 | def verboseprint(*args): 40 | # Print each argument separately so caller doesn't need to 41 | # stuff everything to be printed into a single string 42 | for arg in args: 43 | print(arg, end=" ") 44 | print("") 45 | else: 46 | verboseprint = lambda *a: None # do-nothing function 47 | 48 | class VelocitySmoothing(object): 49 | 50 | def setState(self, a0, v0, x0): 51 | self._accel = a0 52 | self._vel = v0 53 | self._pos = x0 54 | 55 | def __init__(self, a0, v0, x0): 56 | self._jerk = 0.0 57 | self._accel = self._at0 = a0 58 | self._vel = self._vt0 = v0 59 | self._pos = self._xt0 = x0 60 | self._vel_sp = 0.0 61 | self._d = 0 62 | self._t0 = 0.0 63 | 64 | self._max_jerk = 9.0 65 | self._max_accel = 3.0 66 | self._max_vel = 5.0 67 | 68 | self._T1 = 0.0 69 | self._T2 = 0.0 70 | self._T3 = 0.0 71 | 72 | def computeT1(self, a0, v3, j_max, a_max): 73 | delta = 2.0*a0**2 + 4.0*j_max*v3 74 | 75 | if delta < 0.0: 76 | verboseprint('Complex roots\n') 77 | return (0.0, True); 78 | 79 | T1_plus = (-a0 + 0.5*sqrt(delta))/j_max 80 | T1_minus = (-a0 - 0.5*sqrt(delta))/j_max 81 | 82 | verboseprint('T1_plus = {}, T1_minus = {}'.format(T1_plus, T1_minus)) 83 | # Use the solution that produces T1 >= 0 and T3 >= 0 84 | T3_plus = a0/j_max + T1_plus 85 | T3_minus = a0/j_max + T1_minus 86 | 87 | if T1_plus >= 0.0 and T3_plus >= 0.0: 88 | T1 = T1_plus 89 | elif T1_minus >= 0.0 and T3_minus >= 0.0: 90 | T1 = T1_minus 91 | else: 92 | verboseprint("Warning") 93 | T1 = 0.0 94 | 95 | (T1, trapezoidal) = self.saturateT1ForAccel(a0, j_max, T1) 96 | 97 | return (T1, trapezoidal) 98 | 99 | def saturateT1ForAccel(self, a0, j_max, T1): 100 | trapezoidal = False 101 | # Check maximum acceleration, saturate and recompute T1 if needed 102 | a1 = a0 + j_max*T1 103 | if a1 > a_max: 104 | T1 = (a_max - a0) / j_max 105 | trapezoidal = True 106 | elif a1 < -a_max: 107 | T1 = (-a_max - a0) / j_max 108 | trapezoidal = True 109 | 110 | return (T1, trapezoidal) 111 | 112 | def computeT3(self, T1, a0, j_max): 113 | T3 = a0/j_max + T1 114 | 115 | return T3 116 | 117 | def computeT2(self, T1, T3, a0, v3, j_max): 118 | T2 = 0.0 119 | 120 | den = T1*j_max + a0 121 | if abs(den) > FLT_EPSILON: 122 | T2 = (-0.5*T1**2*j_max - T1*T3*j_max - T1*a0 + 0.5*T3**2*j_max - T3*a0 + v3)/den 123 | 124 | return T2 125 | 126 | def updateDurations(self, t): 127 | 128 | if (abs(self._accel) > self._max_accel): 129 | verboseprint("Should be double deceleration profile!") 130 | # Depending of the direction, start accelerating positively or negatively 131 | # For this, we need to predict what would be the velocity at zero acceleration 132 | # because it could be that the current acceleration is too high and that we need 133 | # to start reducing the acceleration directly even if sign(v_d - v_T) < 0 134 | do_stop_check = False 135 | if abs(self._accel) > FLT_EPSILON and do_stop_check: 136 | j_zero_acc = -sign(self._accel) * abs(self._max_jerk); 137 | t_zero_acc = -self._accel / j_zero_acc; 138 | vel_zero_acc = self._vel + self._accel * t_zero_acc + 0.5 * j_zero_acc * t_zero_acc * t_zero_acc; 139 | verboseprint("vel_zero_acc = {}\tt_zero_acc = {}".format(vel_zero_acc, t_zero_acc)) 140 | verboseprint("vel = {}, accel = {}, jerk = {}".format(self._vel, self._accel, j_zero_acc)) 141 | else: 142 | vel_zero_acc = self._vel 143 | 144 | self._d = sign(self._vel_sp - vel_zero_acc) 145 | jerk = self._d*self._max_jerk 146 | 147 | if abs(jerk) < 0.5 * self._max_jerk: 148 | self._T1 = self._T2 = self._T3 = 0.0 149 | print("Return") 150 | return 151 | 152 | self._at0 = self._accel 153 | self._vt0 = self._vel 154 | self._xt0 = self._pos 155 | 156 | delta_v = self._vel_sp - self._vel 157 | # Compute increasing acceleration time 158 | (T1, trapezoidal) = self.computeT1(self._accel, delta_v, jerk, self._max_accel) 159 | # Compute decreasing acceleration time 160 | T3 = self.computeT3(T1, self._accel, jerk); 161 | # Compute constant acceleration time 162 | if trapezoidal: 163 | T2 = self.computeT2(T1, T3, self._accel, delta_v, jerk) 164 | 165 | else: 166 | T2 = 0.0 167 | 168 | self._T1 = T1 169 | self._T2 = T2 170 | self._T3 = T3 171 | self._t0 = t 172 | 173 | def update(self, vel_sp, t): 174 | self._vel_sp = clip(vel_sp, -self._max_vel, self._max_vel) 175 | self.updateDurations(t) 176 | 177 | def evaluatePoly(self, j, a0, v0, x0, t, d): 178 | jt = d*j 179 | at = a0 + jt*t 180 | vt = v0 + a0*t + 0.5*jt*t**2 181 | xt = x0 + v0*t + 0.5*a0*t**2 + 1.0/6.0*jt*t**3 182 | 183 | return (jt, at, vt, xt) 184 | 185 | def evaluateTraj(self, t_now): 186 | """evaluate trajectory for the given time 187 | """ 188 | j = self._max_jerk 189 | d = self._d 190 | t = t_now - self._t0 191 | 192 | if t <= self._T1: 193 | t1 = t 194 | (self._jerk, self._accel, self._vel, self._pos) = self.evaluatePoly(j, self._at0, self._vt0, self._xt0, t1, d) 195 | 196 | elif t <= self._T1 + self._T2: 197 | t1 = self._T1 198 | t2 = t - self._T1 199 | 200 | (self._jerk, self._accel, self._vel, self._pos) = self.evaluatePoly(j, self._at0, self._vt0, self._xt0, t1, d) 201 | (self._jerk, self._accel, self._vel, self._pos) = self.evaluatePoly(0.0, self._accel, self._vel, self._pos, t2, 0.0) 202 | 203 | elif t <= self._T1 + self._T2 + self._T3: 204 | t1 = self._T1 205 | t2 = self._T2 206 | t3 = t - self._T1 - self._T2 207 | 208 | (self._jerk, self._accel, self._vel, self._pos) = self.evaluatePoly(j, self._at0, self._vt0, self._xt0, t1, d) 209 | (self._jerk, self._accel, self._vel, self._pos) = self.evaluatePoly(0.0, self._accel, self._vel, self._pos, t2, 0.0) 210 | (self._jerk, self._accel, self._vel, self._pos) = self.evaluatePoly(j, self._accel, self._vel, self._pos, t3, -d) 211 | 212 | else: 213 | # TODO : do not recompute if the sequence has already been completed 214 | t1 = self._T1 215 | t2 = self._T2 216 | t3 = self._T3 217 | 218 | (self._jerk, self._accel, self._vel, self._pos) = self.evaluatePoly(j, self._at0, self._vt0, self._xt0, t1, d) 219 | (self._jerk, self._accel, self._vel, self._pos) = self.evaluatePoly(0.0, self._accel, self._vel, self._pos, t2, 0.0) 220 | (self._jerk, self._accel, self._vel, self._pos) = self.evaluatePoly(j, self._accel, self._vel, self._pos, t3, -d) 221 | (self._jerk, self._accel, self._vel, self._pos) = self.evaluatePoly(0.0, 0.0, self._vel_sp, self._pos, t - self._T1 - self._T2 - self._T3, 0.0) 222 | 223 | return (self._jerk, self._accel, self._vel, self._pos) 224 | 225 | 226 | if __name__ == '__main__': 227 | # Initial conditions 228 | a0 = 1.18 229 | v0 = 2.52 230 | x0 = 0.0 231 | 232 | # Constraints 233 | j_max = 8.0 234 | a_max = 4.0 235 | v_max = 12.0 236 | 237 | # Simulation time parameters 238 | dt_0 = 0.02 239 | t_end = 6.0 240 | 241 | # Initialize vectors 242 | t = arange (0.0, t_end+dt_0, dt_0) 243 | n = len(t) 244 | 245 | j_T = zeros(n) 246 | a_T = zeros(n) 247 | v_T = zeros(n) 248 | x_T = zeros(n) 249 | v_d = zeros(n) 250 | 251 | j_T[0] = 0.0 252 | a_T[0] = a0 253 | v_T[0] = v0 254 | x_T[0] = x0 255 | v_d[0] = 2.34 256 | 257 | traj = VelocitySmoothing(a0, v0, x0) 258 | traj._max_jerk = j_max 259 | traj._max_accel = a_max 260 | traj._max_vel = v_max 261 | traj._dt = dt_0 262 | 263 | # Main loop 264 | for k in range(0, n): 265 | if k != 0: 266 | if t[k] < 1.0: 267 | v_d[k] = v_d[k-1] 268 | elif t[k] < 1.3: 269 | v_d[k] = 1.0 270 | elif t[k] < 3.0: 271 | v_d[k] = 1.5 272 | else: 273 | v_d[k] = -3.0 274 | verboseprint('k = {}\tt = {}'.format(k, t[k])) 275 | (j_T[k], a_T[k], v_T[k], x_T[k]) = traj.evaluateTraj(t[k]) 276 | traj.update(v_d[k], t[k]) 277 | 278 | verboseprint("T1 = {}\tT2 = {}\tT3 = {}\n".format(traj._T1, traj._T2, traj._T3)) 279 | 280 | plt.plot(t, v_d) 281 | plt.plot(t, j_T) 282 | plt.plot(t, a_T) 283 | plt.plot(t, v_T) 284 | plt.plot(t, x_T, '--') 285 | plt.legend(["v_d", "j_T", "a_T", "v_T", "x_T"]) 286 | plt.xlabel("time (s)") 287 | plt.ylabel("metric amplitude") 288 | plt.show() 289 | -------------------------------------------------------------------------------- /trajectory_generator/closed_loop_ziegler_nichols.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | File: closed_loop_ziegler_nichols.py 6 | Author: Mathieu Bresciani 7 | Email: brescianimathieu@gmail.com 8 | Github: https://github.com/bresch 9 | Description: 10 | Implementation of the Ziegler-Nichols rules for close-loop PID tuning. The gains are given 11 | for a parallel implementation of the PID controller. 12 | Four methods are available: "classical", "overshoot", "no_overshoot" and "pessen". 13 | For more details, see http://notesnet.blogspot.com/2013/02/zieglernichols-method.html 14 | """ 15 | 16 | def compute_PID(K_u, T_u, rule="classical"): 17 | if rule == "classical": 18 | K_p = 0.6*K_u 19 | T_i = 0.5*T_u 20 | T_d = T_u/8.0 21 | 22 | elif rule == "overshoot": 23 | K_p = 0.33*K_u 24 | T_i = 0.5*T_u 25 | T_d = T_u/3.0 26 | 27 | elif rule == "no_overshoot": 28 | K_p = 0.2*K_u 29 | T_i = 0.5*T_u 30 | T_d = T_u/3.0 31 | 32 | elif rule == "pessen": 33 | K_p = 0.7*K_u 34 | T_i = 0.4*T_u 35 | T_d = 0.15*T_u 36 | 37 | return (K_p, T_i, T_d) 38 | 39 | def non_interacting_to_parallel(Kp_n, Ti_n, Td_n): 40 | Kp_p = Kp_n 41 | Ki_p = Kp_n / Ti_n 42 | Kd_p = Kp_n * Td_n 43 | 44 | return (Kp_p, Ki_p, Kd_p) 45 | 46 | def compute_ARW_gain(K_p, K_i, K_d): 47 | K_ARW = 2.0 / K_p 48 | 49 | return K_ARW 50 | 51 | K_u = 0.8 # Ultimate closed-loop gain 52 | T_u = 0.5 # Oscillation period in seconds at critical gain 53 | 54 | rules = ["classical", "overshoot", "no_overshoot", "pessen"] 55 | 56 | for rule in rules: 57 | (Kp_i, Ti_i, Td_i) = compute_PID(K_u, T_u, rule=rule) 58 | (Kp_p, Ki_p, Kd_p) = non_interacting_to_parallel(Kp_i, Ti_i, Td_i) 59 | K_ARW = compute_ARW_gain(Kp_p, Ki_p, Kd_p) 60 | print("Rule \"{}\"\nParallel:\nKp = {}\tKi = {}\tKd = {}\tK_ARW = {}".format(rule, Kp_p, Ki_p, Kd_p, K_ARW)) 61 | print("Non-Interacting:\nKp = {}\tTi = {}\tTd = {}\n".format(Kp_i, Ti_i, Td_i)) 62 | -------------------------------------------------------------------------------- /trajectory_generator/images/vel_traj_graph.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Auterion/Flight_Control_Prototyping_Scripts/6011414519b20478a011e79c620c25413b80a8cf/trajectory_generator/images/vel_traj_graph.png -------------------------------------------------------------------------------- /trajectory_generator/velocity_trajectory_generator.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | File: velocity_trajectory_generator.py 6 | Author: Mathieu Bresciani 7 | Email: brescianimathieu@gmail.com 8 | Github: https://github.com/bresch 9 | Description: 10 | Given a desired velocity setpoint v_d, the trajectory generator computes 11 | a time-optimal trajectory satisfaying the following variable constraints: 12 | - j_max : maximum jerk 13 | - a_max : maximum acceleration 14 | - v_max : maximum velocity 15 | - a0 : initial acceleration 16 | - v0 : initial velocity 17 | - v3 : final velocity 18 | The hard constraint used to generate the optimizer is: 19 | - a3 = 0.0 : final acceleration 20 | 21 | The trajectory generated is made by three parts: 22 | 1) Increasing acceleration during T1 seconds 23 | 2) Constant acceleration during T2 seconds 24 | 3) Decreasing acceleration during T3 seconds 25 | 26 | This script also generates a position setpoint which is computed 27 | as the integral of the velocity setpoint. If this one is bigger than 28 | x_err_max, the integration is frozen. 29 | 30 | The algorithm is simulated in a loop (typically that would be the position control loop) 31 | where the trajectory is recomputed a each iteration. 32 | """ 33 | 34 | from __future__ import print_function 35 | 36 | from numpy import * 37 | import matplotlib.pylab as plt 38 | import sys 39 | import math 40 | 41 | FLT_EPSILON = sys.float_info.epsilon 42 | NAN = float('nan') 43 | verbose = False 44 | 45 | if verbose: 46 | def verboseprint(*args): 47 | # Print each argument separately so caller doesn't need to 48 | # stuff everything to be printed into a single string 49 | for arg in args: 50 | print(arg, end=" ") 51 | print("") 52 | else: 53 | verboseprint = lambda *a: None # do-nothing function 54 | 55 | 56 | def integrate_T(j, a_prev, v_prev, x_prev, dt, a_max, v_max): 57 | 58 | a_T = j * dt + a_prev 59 | 60 | 61 | #v_T = j*dt*dt/2.0 + a_prev*dt + v_prev # Original equation: 3 mult + 1 div + 2 add 62 | v_T = dt/2.0 * (a_T + a_prev) + v_prev # Simplification using a_T: 1 mult + 1 div + 2 add 63 | 64 | #x_T = j*dt*dt*dt/6.0 + a_prev*dt*dt/2.0 + v_prev*dt + x_prev # Original equation: 6 mult + 2 div + 3 add 65 | x_T = dt/3.0 * (v_T + a_prev*dt/2.0 + 2*v_prev) + x_prev # Simplification using v_T: 3 mult + 2 div + 3 add 66 | 67 | return (a_T, v_T, x_T) 68 | 69 | def recomputeMaxJerk(a0, v3, T1, j_max): 70 | 71 | # If T1 is smaller than dt, it means that the jerk is too large to reach the 72 | # desired acceleration with a bang-bang signal => recompute the maximum jerk 73 | 74 | verboseprint(a0, v3, T1) 75 | delta = 2.0*T1**2*a0**2 - 4.0*T1*a0*v3 + v3**2 76 | if delta < 0.0: 77 | return 0.0 78 | j_new_minus = -0.5*(2.0*T1*a0 - v3)/T1**2 - 0.5*sqrt(delta)/T1**2 79 | j_new_plus = -0.5*(2.0*T1*a0 - v3)/T1**2 + 0.5*sqrt(delta)/T1**2 80 | verboseprint('j_new_plus = {}, j_new_minus = {}'.format(j_new_plus, j_new_minus)) 81 | (T1_plus, T3_plus) = compute_T1_T3(a0, v3, j_new_plus) 82 | (T1_minus, T3_minus) = compute_T1_T3(a0, v3, j_new_minus) 83 | if T1_plus >= 0.0 and T3_plus >= 0.0: 84 | j_new = j_new_plus 85 | if T1_minus >= 0.0 and T3_minus >= 0.0: 86 | verboseprint('Both jerks are valid; check for time optimality') 87 | if T1_plus + T3_plus > T1_minus + T3_minus: 88 | j_new = j_new_minus 89 | elif T1_minus >= 0.0 and T3_minus >= 0.0: 90 | j_new = j_new_minus 91 | if T1_plus >= 0.0 and T3_plus >= 0.0: 92 | verboseprint('Both jerks are valid; check for optimality') 93 | if T1_plus + T3_plus < T1_minus + T3_minus: 94 | j_new = j_new_plus 95 | else: 96 | verboseprint('Error in recomputeMaxJerk') 97 | j_new = j_max 98 | 99 | return j_new 100 | 101 | def compute_T1_T3(a0, v3, j_max): 102 | 103 | delta = 2.0*a0**2 + 4.0*j_max*v3 104 | if delta < 0.0: 105 | verboseprint('Complex roots\n') 106 | return (0.0, j_max); 107 | 108 | T1_plus = (-a0 + 0.5*sqrt(delta))/j_max 109 | T1_minus = (-a0 - 0.5*sqrt(delta))/j_max 110 | 111 | verboseprint('T1_plus = {}, T1_minus = {}'.format(T1_plus, T1_minus)) 112 | # Use the solution that produces T1 >= 0 and T3 >= 0 113 | T3_plus = a0/j_max + T1_plus 114 | T3_minus = a0/j_max + T1_minus 115 | 116 | if T1_plus >= 0.0 and T3_plus >= 0.0: 117 | T1 = T1_plus 118 | T3 = T3_plus 119 | elif T1_minus >= 0.0 and T3_minus >= 0.0: 120 | T1 = T1_minus 121 | T3 = T3_minus 122 | else: 123 | T1 = 0.0 124 | T3 = NAN 125 | 126 | return (T1, T3) 127 | 128 | def compute_T1(a0, v3, j_max, a_max, dt): 129 | 130 | (T1, T3) = compute_T1_T3(a0, v3, j_max) 131 | j_max_T1 = j_max 132 | 133 | if T1 < dt or T1 < 0.0: 134 | return (0.0, j_max) 135 | 136 | # Check maximum acceleration, saturate and recompute T1 if needed 137 | a1 = a0 + j_max_T1*T1 138 | if a1 > a_max: 139 | T1 = (a_max - a0) / j_max_T1 140 | elif a1 < -a_max: 141 | T1 = (-a_max - a0) / j_max_T1 142 | 143 | return (T1, j_max_T1) 144 | 145 | 146 | def computeT1_T123(T123, a0, v3, j_max, dt): 147 | delta = T123**2*j_max**2 + 2.0*T123*a0*j_max - a0**2 - 4.0*j_max*v3 148 | 149 | if delta < 0.0: 150 | verboseprint("WARNING delta = {}".format(delta)) 151 | j_max = -j_max 152 | delta = T123**2*j_max**2 + 2.0*T123*a0*j_max - a0**2 - 4.0*j_max*v3 153 | 154 | sqrt_delta = sqrt(delta); 155 | 156 | if abs(j_max) > FLT_EPSILON: 157 | denominator_inv = 1.0 / (2.0 * j_max); 158 | else: 159 | verboseprint("WARNING : j_max = {}".format(j_max)) 160 | return 0.0 161 | 162 | b = -T123 * j_max + a0 163 | 164 | T1_plus = (-b + sqrt_delta) * denominator_inv; 165 | T1_minus = (-b - sqrt_delta) * denominator_inv; 166 | verboseprint("plus = {}, minus = {}".format(T1_plus, T1_minus)) 167 | T1_plus = max(T1_plus, 0.0) 168 | T1_minus = max(T1_minus, 0.0) 169 | (T3_plus, j_max_T3) = compute_T3(T1_plus, a0, v3, j_max, dt) 170 | (T3_minus, j_max_T3) = compute_T3(T1_minus, a0, v3, j_max, dt) 171 | if (T1_plus + T3_plus > T123): 172 | T1 = T1_minus 173 | elif (T1_minus + T3_minus > T123): 174 | T1 = T1_plus 175 | else: 176 | T1 = 0.0 177 | 178 | verboseprint("plus = {}, minus = {}".format(T1_plus, T1_minus)) 179 | 180 | if T1 < dt: 181 | T1 = 0.0 182 | 183 | return (T1, j_max) 184 | 185 | def compute_T3(T1, a0, v3, j_max, dt): 186 | T3 = a0/j_max + T1 187 | j_max_T3 = j_max 188 | 189 | if T1 < FLT_EPSILON and T3 < dt and T3 > FLT_EPSILON: 190 | # Force T3 to be the size of dt 191 | verboseprint('Exact T3 = {}'.format(T3)) 192 | T3 = dt 193 | verboseprint('New T3 = {}'.format(T3)) 194 | 195 | # Adjust new max jerk for adjusted T3 196 | # such that the acceleration can go from a0 197 | # to 0 in a single step (T3 = dt) 198 | j_max_T3 = a0/T3 199 | verboseprint('Full jerk = {}'.format(j_max)) 200 | if abs(j_max_T3) > abs(j_max): 201 | j_max_T3 = j_max 202 | verboseprint("Warning: jerk is too large") 203 | 204 | T3 = max(T3, 0.0) 205 | return (T3, j_max_T3) 206 | 207 | def compute_T2(T1, T3, a0, v3, j_max, dt): 208 | T2 = 0.0 209 | 210 | den = T1*j_max + a0 211 | if abs(den) > FLT_EPSILON: 212 | T2 = (-0.5*T1**2*j_max - T1*T3*j_max - T1*a0 + 0.5*T3**2*j_max - T3*a0 + v3)/den 213 | 214 | if T2 < dt: 215 | T2 = 0.0 216 | 217 | return T2 218 | 219 | def compute_T2_T123(T123, T1, T3): 220 | T2 = T123 - T1 - T3 221 | 222 | if T2 < dt: 223 | T2 = 0.0 224 | 225 | return T2 226 | 227 | # ============================================================ 228 | # ============================================================ 229 | 230 | # Initial conditions 231 | a0 = 0.0 232 | v0 = 0.5 233 | x0 = 0.0 234 | 235 | # Constraints 236 | j_max = 9.0 237 | a_max = 6.0 238 | v_max = 6.0 239 | 240 | # Simulation time parameters 241 | dt_0 = 1.0/50.0 242 | t_end = 5.2 243 | 244 | # Initialize vectors 245 | t = arange (0.0, t_end+dt_0, dt_0) 246 | n = len(t) 247 | 248 | j_T = zeros(n) 249 | j_T_corrected = zeros(n) 250 | a_T = zeros(n) 251 | v_T = zeros(n) 252 | x_T = zeros(n) 253 | v_d = zeros(n) 254 | 255 | j_T[0] = 0.0 256 | j_T_corrected[0] = 0.0 257 | a_T[0] = a0 258 | v_T[0] = v0 259 | x_T[0] = x0 260 | v_d[0] = 0.0 261 | 262 | dt_prev = dt_0 263 | sigma_jitter = 0.0 264 | sigma_jitter = dt_0/5.0 265 | 266 | # Main loop 267 | for k in range(0, n): 268 | dt = dt_0 + random.randn() * sigma_jitter # Add jitter 269 | 270 | if k > 0: 271 | t[k] = t[k-1] + dt 272 | verboseprint('k = {}\tt = {}'.format(k, t[k])) 273 | 274 | # Correct the jerk if dt is bigger than before and that we only need one step of jerk to complete phase T1 or T3 275 | # This helps to avoid overshooting and chattering around zero acceleration due to dt jitter 276 | if dt > dt_prev \ 277 | and ( \ 278 | (dt > T1 and T1 > FLT_EPSILON) \ 279 | or (dt > T3 and T3 > FLT_EPSILON)): 280 | j_T_corrected[k-1] = j_T[k-1] * dt_prev / dt 281 | else: 282 | j_T_corrected[k-1] = j_T[k-1] 283 | 284 | # Integrate the trajectory 285 | (a_T[k], v_T[k], x_T[k]) = integrate_T(j_T_corrected[k-1], a_T[k-1], v_T[k-1], x_T[k-1], dt, a_max, v_max) 286 | 287 | # Change the desired velocity (simulate user RC sticks) 288 | if t[k] < 3.0: 289 | v_d[k] = v_d[0] 290 | elif t[k] < 4.5: 291 | v_d[k] = 4.0 292 | else: 293 | v_d[k] = 5.0 294 | 295 | # Depending of the direction, start accelerating positively or negatively 296 | # For this, we need to predict what would be the velocity at zero acceleration 297 | # because it could be that the current acceleration is too high and that we need 298 | # to start reducing the acceleration directly even if sign(v_d - v_T) < 0 299 | if abs(a_T[k]) > FLT_EPSILON: 300 | j_zero_acc = -sign(a_T[k]) * abs(j_max); 301 | t_zero_acc = -a_T[k] / j_zero_acc; 302 | vel_zero_acc = v_T[k] + a_T[k] * t_zero_acc + 0.5 * j_zero_acc * t_zero_acc * t_zero_acc; 303 | verboseprint("vel_zero_acc = {}\tt_zero_acc = {}".format(vel_zero_acc, t_zero_acc)) 304 | else: 305 | vel_zero_acc = v_T[k] 306 | 307 | #if v_d[k] > v_T[k]: 308 | if v_d[k] > vel_zero_acc : 309 | j_max = abs(j_max) 310 | else: 311 | j_max = -abs(j_max) 312 | 313 | (T1, j_max_T1) = compute_T1(a_T[k], v_d[k] - v_T[k], j_max, a_max, dt) 314 | 315 | (T3, j_max_T1) = compute_T3(T1, a_T[k], v_d[k] - v_T[k], j_max_T1, dt) 316 | 317 | T2 = compute_T2(T1, T3, a_T[k], v_d[k] - v_T[k], j_max_T1, dt) 318 | 319 | verboseprint("T1 = {}\tT2 = {}\tT3 = {}\n".format(T1, T2, T3)) 320 | 321 | # Apply correct jerk (min, max or zero) 322 | if T1 > FLT_EPSILON: 323 | j_T[k] = j_max_T1 324 | elif T2 > FLT_EPSILON: 325 | j_T[k] = 0.0 326 | elif T3 > FLT_EPSILON: 327 | j_T[k] = -j_max_T1 328 | else: 329 | j_T[k] = 0.0 330 | verboseprint('T123 = 0, t = {}\n'.format(t[k])) 331 | 332 | dt_prev = dt 333 | 334 | # end loop 335 | 336 | 337 | verboseprint('=========== END ===========') 338 | # Plot trajectory and desired setpoint 339 | plt.plot(t, v_d) 340 | plt.plot(t, j_T, '*') 341 | plt.plot(t, a_T, '*') 342 | plt.plot(t, v_T) 343 | plt.plot(t, x_T) 344 | plt.plot(arange (0.0, t_end+dt_0, dt_0), t) 345 | plt.plot(t, j_T_corrected) 346 | plt.legend(["v_d", "j_T", "a_T", "v_T", "x_T", "t"]) 347 | plt.xlabel("time (s)") 348 | plt.ylabel("metric amplitude") 349 | plt.show() 350 | 351 | # Time sync tests 352 | dt = 0.01 353 | T123 = dt 354 | a0 = 6.58e-6 355 | v3 = 0.00049 356 | j_max = -55.2 357 | (T1, j_max_T1) = computeT1_T123(T123, a0, v3, j_max, dt) 358 | (T3, j_max_T3) = compute_T3(T1, a0, v3, j_max_T1, dt) 359 | T2 = compute_T2_T123(T123, T1, T3) 360 | verboseprint("T123 = {}\tT1 = {}\tT2 = {}\tT3 = {}".format(T123, T1, T2, T3)) 361 | verboseprint("j_max_T3 = {}".format(j_max_T3)) 362 | -------------------------------------------------------------------------------- /trajectory_generator/velocity_trajectory_generator_symbolic.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | File: velocity_trajectory_generator_symbolic.py 6 | Author: Mathieu Bresciani 7 | Email: brescianimathieu@gmail.com 8 | Github: https://github.com/bresch 9 | Description: 10 | Derivation of a limited jerk time optimal velocity-driven 11 | trajectory generator. 12 | """ 13 | 14 | from sympy import * 15 | 16 | j = Symbol("j_max", real=True) 17 | a_0 = Symbol("a0", real=True) 18 | a_3 = Symbol("a3", real=True) 19 | v_0 = Symbol("v0", real=True) 20 | v_3 = Symbol("v3", real=True) 21 | x_0 = Symbol("x0", real=True) 22 | T_1 = Symbol("T1", real=True) 23 | T_2 = Symbol("T2", real=True) 24 | T_3 = Symbol("T3", real=True) 25 | a_max = Symbol("a_max", real=True) 26 | 27 | # Final acceleration (at t3) 28 | f1 = a_0 + j*T_1 - j*T_3 - a_3 29 | f1 = f1.subs(a_3, 0) # Force final acceleration to be null 30 | res_T3 = solve(f1, T_3) 31 | res_T3 = res_T3[0] 32 | # Acceleration at t1 33 | at1 = a_0 + j*T_1 34 | # Acceleration at t2 35 | at2 = at1 36 | # Velocity at t1 37 | vt1 = v_0 + a_0*T_1 + 0.5*j*T_1**2 38 | # Velocity at t2 39 | vt2 = vt1 + at1*T_2 40 | # Velocity at t3 41 | vt3 = vt2 + at2*T_3 - 0.5*j*T_3**2 42 | # Position at t1 43 | xt1 = x_0 + v_0*T_1 + 0.5*a_0*T_1**2 + 1/6*j*T_1**3 44 | # Position at t2 45 | xt2 = xt1 + vt1*T_2 + 0.5*at1*T_2**2 46 | # Position at t3 47 | xt3 = xt2 + vt2*T_3 + 0.5*at2*T_3**2 - 1/6*j*T_3**3 48 | 49 | f2 = vt3 - v_3 50 | f2 = f2.subs([(v_0, 0)]) 51 | print("Step 1 - Setting T_2 = 0, compute T_1 as follows:") 52 | res_T1 = solve(f2.subs([(T_2, 0), (T_3, res_T3)]), T_1) 53 | res_T1 = res_T1[0] 54 | print("T_1 =") 55 | pprint(res_T1) 56 | print(res_T1) 57 | print("If jerk is too large, recompute it for fixed T1. New jerk = {}") 58 | res_j1 = solve(f2.subs([(T_2, 0), (T_3, res_T3)]), j) 59 | pprint(res_j1[0]) 60 | print(res_j1[0]) 61 | res1_T1 = solve(f1, T_1) 62 | res1_T1 = res1_T1[0] 63 | res_j3 = solve(f2.subs([(T_2, 0), (T_1, res1_T1)]), j) 64 | print("If jerk is too large, and T1 = 0. New jerk = {}") 65 | pprint(res_j3[0]) 66 | print(res_j3[0]) 67 | print("Step 2 - Check for saturation. If a_0 + j*T_1 > a_max, recompute T_1 using:") 68 | print("T_1 =") 69 | pprint(solve(a_0 + j*T_1 - a_max, T_1)[0]) 70 | print(solve(a_0 + j*T_1 - a_max, T_1)[0]) 71 | print("Step 3 - Compute T3 using:") 72 | print("T_3 =") 73 | pprint(res_T3) 74 | print(res_T3) 75 | res_T2 = solve(f2, T_2) 76 | res_T2 = res_T2[0] 77 | print("Step 3 - Finally compute the required constant acceleration part using:") 78 | print("T_2 =") 79 | pprint(res_T2) 80 | print(res_T2) 81 | 82 | dt = Symbol("dt", real=True) 83 | j_max = Symbol("j_max", real=True) 84 | x_0 = Symbol("x_0", real=True) 85 | j_sp = Symbol("j_sp", real=True) 86 | a_sp = Symbol("a_sp", real=True) 87 | v_sp = Symbol("v_sp", real=True) 88 | 89 | print("=============== Generate the trajectory ===============") 90 | f_j_sp = j_max 91 | f_a_sp = integrate(f_j_sp, dt) + a_0 92 | f_v_sp = integrate(f_a_sp, dt) + v_0 93 | f_x_sp = integrate(f_v_sp, dt) + x_0 94 | print("j_sp =") 95 | pprint(f_j_sp) 96 | print("a_sp =") 97 | f_a_sp_subs = f_a_sp.subs(f_j_sp, j_sp) 98 | pprint(f_a_sp_subs) 99 | print("v_sp =") 100 | f_v_sp_subs = f_v_sp.subs(f_a_sp, a_sp) 101 | pprint(f_v_sp_subs) 102 | print("x_sp =") 103 | pprint(f_x_sp) 104 | 105 | print("=============== Time synchronization ===============") 106 | T123 = Symbol("T123", real=True) # Total time 107 | 108 | f3 = T123 - T_1 - T_2 - T_3 # Time constraint 109 | res_T2 = solve(f3.subs(T_3, res_T3), T_2) 110 | res_T2 = res_T2[0] 111 | res_T1 = solve(f2.subs([(T_2, res_T2), (T_3, res_T3)]), T_1) 112 | res_T1 = res_T1 113 | print("For multiple axes applications, we need to synchronize the trajectories such that they all end at the same time.\n\ 114 | Given the total time of the longest trajectory T, we can solve again T_1, T_2 and T_3 for the other axes:") 115 | print("T_1 =") 116 | pprint(res_T1) 117 | print(res_T1) 118 | print("T_3 =") 119 | pprint(res_T3) 120 | print(res_T2) 121 | print("T_2 =") 122 | pprint(solve(f3, T_2)[0]) 123 | --------------------------------------------------------------------------------