Performance improvement to least-squares based contrained control allocation #3036

tblaha · 2023-06-12T14:52:03Z

As part of my masters thesis, I implemented optimized least-squares solvers for Weighted Least-Squares control allocation with bound-constraints on the actuators. To achieve significant improvements, the matrix factors are updated instead of re-computed at each iteration of the solver. Additionally, sparsity of the matrices are exploited, and "warm-starting" is implemented (initializing with the solution at the previous sample).

This PR contains

the algorithms, packaged in math/ActiveSetControlAlloc (QR_naive is largely identical to the previous WLS allocator, plus some fixes)
changes to stabilization_indi.c
necessary configuration options in the INDI module config
a hexacopter airframe that has been used for the tests, along with its JSBsim simulation model ("Flamewheel")
flight plans used to test the control allocation performance
a new pprz message to obtain status and performance of the allocator

To Do:

rebase to new master
check default config options again, and improve descriptions
Testflight with the hexacopter to verify that everything still works (masters thesis test flights were with an older version of master)

I would appreciate some advice of what of this should/should not go into this PR to master!

tblaha · 2023-06-14T12:21:31Z

test flight successful. All algorithms and settings work as expected. I made QR the default which is not as fast as CHOL, does not suffer from numerical issues. CHOL also did not crash my platform, but is known to provide less accurate solutions when cond_bound is too high.

gautierhattenberger

Thank you trying to improve this solver. I have however several remarks to make this code easier to read and to maintain. It would also be good to avoid duplication of code from the math lib, or even integrate the new one to make it easier to use by other people.

gautierhattenberger · 2023-06-14T16:04:21Z

.vscode/c_cpp_properties.json

@@ -1,22 +0,0 @@
-{
-    "configurations": [


there is no reason to the vscode files

gautierhattenberger · 2023-06-14T16:07:02Z

conf/messages.xml

@@ -0,0 +1,3354 @@
+<?xml version="1.0"?>
+<!DOCTYPE protocol SYSTEM "messages.dtd">


This file should be removed, only use it for your own testing. If you need to include a new message, make a PR in pprzlink and update the submodule in paparazzi.

gautierhattenberger · 2023-06-14T16:10:43Z

conf/simulator/jsbsim/aircraft/flamewheel.xml

+      >>> np.array([5*np.sin(th)]).T + np.array([1.9685, -1.9685])
+    -->
+
+    <force name="fr_couple1" frame="BODY" unit="LBS">


You should consider replace these pairs of forces by moment like this: https://github.com/paparazzi/paparazzi/blob/master/conf/simulator/jsbsim/aircraft/simple_quad_wind.xml#L234

gautierhattenberger · 2023-06-14T16:11:33Z

conf/telemetry/tudelft/ctl_alloc_algos.xml

+<telemetry>
+
+
+  <process name="Main">


Maybe it is not needed to include this file in the PR.

gautierhattenberger · 2023-06-14T16:13:02Z

sw/airborne/boards/px4fmu/chibios/v5.0/board.h

@@ -57,7 +57,7 @@
 #endif

 /* Default powerbrick values */
-#define DefaultVoltageOfAdc(adc) ((3.3f/4096.0f) * 10.3208191126f * adc)
+#define DefaultVoltageOfAdc(adc) ((3.3f/4096.0f) * 18.0f * adc)


It's very different from original value. Was it really wrong before ? Or is it specific to your board ?

It was not wrong before. A different value should IMHO have been set in airframe file like:

<section name="BAT"> ...  <define name="VoltageOfAdc(_adc)" value="(3.3f/4096.0f) * 18.0f * adc"/> ...

gautierhattenberger · 2023-06-28T13:36:21Z

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h

@@ -26,6 +26,10 @@
 #include "firmwares/rotorcraft/stabilization.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h"
+#include "math/ActiveSetCtlAlloc/src/common/solveActiveSet.h"
+#ifdef USE_CHIBIOS_RTOS
+#include <ch.h>


I would like to avoid including OS specific code here, are you sure it is really needed ?

gautierhattenberger · 2023-06-28T13:38:04Z

sw/airborne/math/ActiveSetCtlAlloc/src/common/setupWLS.c

+#include "setupWLS.h"
+#include <math.h>
+
+void setupWLS_A(


Please format the code (and the rest of this lib). There is a script for that at the root of the project (fix_code_style.sh).

gautierhattenberger · 2023-06-28T13:38:38Z

sw/airborne/math/ActiveSetCtlAlloc/src/common/setupWLS.c

+
+    *cond_est = *max_sig / ((min_diag2 > 1e-10) ? min_diag2 : 1e-10);
+
+}


missing end of line

gautierhattenberger · 2023-06-28T13:42:00Z

sw/airborne/math/ActiveSetCtlAlloc/src/lib/chol_math.c

+#include <math.h>
+#include <stdint.h>
+
+void pprz_cholesky_float(num_t **out, num_t **in, num_t* inv_diag, int n)


Why not using the function that is already in the math lib ? In general, why the different solvers are not at the root of the math folder if they are generic (cholesky, qr, ...) ?

gautierhattenberger · 2023-06-28T13:44:13Z

sw/airborne/math/ActiveSetCtlAlloc/src/lib/qr_solve/qr_solve.c

+ * This is part of the qr_solve library from John Burkardt.
+ * http://people.sc.fsu.edu/~jburkardt/c_src/qr_solve/qr_solve.html
+ *
+ * It is slightly modified to make it compile on simple microprocessors,


is this solver really more efficient than the SVD-based solver already in the math lib math/pprz_matrix_decomp_float.h ?

tblaha added 4 commits June 13, 2023 13:08

initial merge, definitely doesnt compile

2612d19

update module conf, still untested

57f1c6c

works flawlessly in sim

328d5ca

added all components

1e6bacf

tblaha force-pushed the updating_ctl_alloc_algos_PR branch from 0025f3e to 1e6bacf Compare June 13, 2023 11:17

update defaults

37dc8a8

gautierhattenberger requested changes Jun 28, 2023

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Performance improvement to least-squares based contrained control allocation #3036

Performance improvement to least-squares based contrained control allocation #3036

tblaha commented Jun 12, 2023 •

edited

tblaha commented Jun 14, 2023

gautierhattenberger left a comment

gautierhattenberger Jun 14, 2023

gautierhattenberger Jun 14, 2023

gautierhattenberger Jun 14, 2023

gautierhattenberger Jun 14, 2023

gautierhattenberger Jun 14, 2023

OpenUAS Oct 25, 2023

gautierhattenberger Jun 28, 2023

gautierhattenberger Jun 28, 2023

gautierhattenberger Jun 28, 2023

gautierhattenberger Jun 28, 2023

gautierhattenberger Jun 28, 2023

		@@ -0,0 +1,3354 @@
		<?xml version="1.0"?>
		<!DOCTYPE protocol SYSTEM "messages.dtd">


		cond_est = max_sig / ((min_diag2 > 1e-10) ? min_diag2 : 1e-10);

		}

Performance improvement to least-squares based contrained control allocation #3036

Are you sure you want to change the base?

Performance improvement to least-squares based contrained control allocation #3036

Conversation

tblaha commented Jun 12, 2023 • edited

tblaha commented Jun 14, 2023

gautierhattenberger left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

tblaha commented Jun 12, 2023 •

edited