[WIP] use vehicle_angular_velocity instead of vehicle_attitude

[dayjaby]

Since 2 years, PX4 removed the roll/pitch/yaw rates from vehicle_attitude. With this PR we use the new uORB topics.

Some test flights resulted in this:

Best fit = 0.7417820116525093
Identified Transfer function:  Continuous-Time Transfer function
 1 input and 1 output

  Poles(real)    Poles(imag)    Zeros(real)    Zeros(imag)
-------------  -------------  -------------  -------------
     -3.71677        3.90951       -0.54223              0
     -3.71677       -3.90951


 [SYSID] WARNING Poor fitting 0.7417820116525093. Try different data window with more dynamics!
Not proceeding with control tuning.

What exactly does it want me to do?

For a log file with better fittings, we get an error at the end:

Best fit = 0.8441755549191987
Identified Transfer function:  Continuous-Time Transfer function
 1 input and 1 output

  Poles(real)    Poles(imag)    Zeros(real)    Zeros(imag)
-------------  -------------  -------------  -------------
     -6.11466              0       -2.41642              0
    -17.1075               0


 --------------- Finding optimal gains using Genetic Optimization ---------------
 [GA] Start of evolution
 [GA] evaluating 10 generations
 [GA] -------------------------- Generation 1 --------------------------
 [GA] -------------------------- Generation 2 --------------------------
 [GA] -------------------------- Generation 3 --------------------------
 [GA] -------------------------- Generation 4 --------------------------
 [GA] -------------------------- Generation 5 --------------------------
 [GA] -------------------------- Generation 6 --------------------------
 [GA] -------------------------- Generation 7 --------------------------
 [GA] -------------------------- Generation 8 --------------------------
 [GA] -------------------------- Generation 9 --------------------------
 [GA] -------------------------- Generation 10 --------------------------
 [GA] -- End of (successful) evolution --
optimal q [786.3392089629161, 765.43445952877, 807.7649339681285]
optimal r 477.9803354147315
Traceback (most recent call last):
  File "px4_pid_tuner.py", line 908, in <module>
    main(args)
  File "px4_pid_tuner.py", line 864, in main
    print("optimal {} rate PID gains {}".format(args["axis"], ga.get_gains()))
  File "px4_pid_tuner.py", line 775, in get_gains
    self._pid.pid_design()
  File "px4_pid_tuner.py", line 497, in pid_design
    [ self._C.dot(self._A) - (self._C.dot(self._B)).dot(Kp_bar) ]
ValueError: operands could not be broadcast together with shapes (1,2) (1,1,3)

Used python versions:

• numpy 1.21.2
• pandas 1.3.2
• etc

[dayjaby]

@mzahana Your implementation is based on https://arxiv.org/pdf/1301.0931.pdf, right? Do you have any notes on your implementation, e.g. what's the mathematical equivalence of

        C_bar = np.block([
                            [ self._C ],
                            [ self._C.dot(self._A) - (self._C.dot(self._B)).dot(Kp_bar) ]
                        ])

?

[mzahana]

@dayjaby Thank you very much for this PR. I rarely check/maintain this package. I personally have not had a chance to use this package effectively with real vehicles as I was not getting good gains. I am not sure what was the issue at that time. However, I kept the package in order to hopefully improve. The reference I mentioned in the code does not seem to be available anymore ! here. I will try to dig and find it, and get back to you.

For the error, it seems dimension mismatch, I am not sure if different SIPPY version would cause this issue. Again, it's been a while for me since I touched this pkg, I need to refresh and re-run things to be able to help. Thank you

[dayjaby]

Do you remember the name of the paper?

I would happily help testing this with newer packages/px4 versions. Just need to understand the maths ^^

[mzahana]

@dayjaby Here are some references I used

1. Ref1, section C
2. Ref2 referenced in [1]