Incremental Nonlinear Dynamic Inversion (INDI) position controller for the Crazyflie #568

volmen3 · 2020-03-29T22:04:56Z

Hello, Dear Bitcraze community, this is a pull request for another version of the position controller, known as Incremental Nonlinear Dynamic Inversion (INDI). This controller works as the outer loop of the already existing INDI attitude controller (inner loop). It was implemented in the framework of my semester thesis under the supervision of @EwoudSmeur and according to the following publication:
Ewoud J. J. Smeur, Guido C. H. E. de Croon, Qiping Chu "Cascaded Incremental Nonlinear Dynamic Inversion Control for MAV Disturbance Rejection"

Below are some notes regarding the implementation.

Files that have been modified:
Makefile
src/modules/src/stabilizer.c (changed controller type)
src/modules/interface/controller_indi.h
src/modules/src/controller_indi.c (corrected some of the mistakes, implemented call of the position controller)

Files that have been added:
src/modules/interface/position_controller_indi.h
src/modules/src/position_controller_indi.c

Unfortunately, I could not find a clear information/documentation about different modes and their influence on the behavior of the Crazyflie. So I implemented the control inputs (reference position) to be set in the "Parameters" section of the cfclient. If the current handling of the control inputs does not correspond to some of the internal guidelines, feel free to adjust it.
The INDI attitude controller works as before just right after the start of the Crazyflie, to activate the outer loop (position controller), the variable outerLoopActive (ctrlINDI group) has to be manually set to 1. When the outer loop is active, the position controller is still in the waiting mode (this is done to avoid random oscillations of the thrust which happen if the position estimates are not accurate, e.g due to homogeneous pattern of the surface CF is placed on). So to activate the controller the arm variable (INDI_Outer group) has to be set to 1. Now, when outerLoopActive and arm are set to 1, the position can be commanded through the following variables (INDI_Outer group): pos_set_x, pos_set_y, pos_set_z. After the landing, the arm variable has to be set back to 0. To return to the mode, where only the INDI attitude controller is active, set outerLoopActive to 0.

knmcguire · 2020-03-30T10:02:18Z

Hi and thanks for your pull request! Happy to finally get INDI in the position control as well! I will review the code in detail later this week and try it for myself.

I just see a little something already, as the INDI controller is now defined as standard in your pull request. I will put a remark at the line of code. I will give the rest of the review later this week

knmcguire · 2020-03-30T10:04:09Z

src/modules/src/stabilizer.c

@@ -184,7 +184,7 @@ void stabilizerInit(StateEstimatorType estimator)

  sensorsInit();
  stateEstimatorInit(estimator);
-  controllerInit(ControllerTypeAny);


Could you please return this to ControllerTypeAny? The controller type will be set later in the stabilizer.c code

thanks! more comments are now coming up

knmcguire · 2020-03-31T13:40:10Z

Makefile

@@ -168,7 +168,7 @@ PROJ_OBJ += crtp_commander_generic.o crtp_localization_service.o
 PROJ_OBJ += attitude_pid_controller.o sensfusion6.o stabilizer.o
 PROJ_OBJ += position_estimator_altitude.o position_controller_pid.o
 PROJ_OBJ += estimator.o estimator_complementary.o
-PROJ_OBJ += controller.o controller_pid.o controller_mellinger.o controller_indi.o


Could you add the position_controller_indi.o to line 169 instead, so that all position controllers are in the Makefile together?

knmcguire · 2020-03-31T13:41:16Z

src/modules/interface/controller_indi.h

-#define STABILIZATION_INDI_G1_P 0.0034724f
-#define STABILIZATION_INDI_G1_Q 0.0052575f
-#define STABILIZATION_INDI_G1_R -0.0015942f
-#define STABILIZATION_INDI_G2_R -0.11281f
-#define STABILIZATION_INDI_REF_ERR_P 3.57f
-#define STABILIZATION_INDI_REF_ERR_Q 3.57f
-#define STABILIZATION_INDI_REF_ERR_R 3.57f


This is quite a difference. How did you tune this? I guess this are the values for the INDI attitude controller right?

The attitude INDI was quite uncontrollable (very slow and not very stable), that's why we have decided to re-estimate some of the parameters.
The gains (STABILIZATION_INDI_REF_ERR_P/Q/R) were tuned manually. The control effectiveness parameters (G1, G2) of the INDI were tuned according to the paper "Adaptive Incremental Nonlinear Dynamic Inversion for Attitude Control of Micro Air Vehicles" (Section A. Parameter Estimation). I have uploaded two Matlab scripts (param_estimation.m, import_logdata.m) and the flight data (data_1901.csv) I used to estimate these parameters. They are located in the /tools/param_est folder.

knmcguire · 2020-03-31T13:45:39Z

src/modules/src/controller_indi.c

@@ -149,7 +155,7 @@ void controllerINDI(control_t *control, setpoint_t *setpoint,
 		}
 	}

-	if (RATE_DO_EXECUTE(POSITION_RATE, tick)) {
+	if (RATE_DO_EXECUTE(POSITION_RATE, tick) && !outerLoopActive) {


So I'm not exactly sure about this. So you made an outerloop boolean to give people the choice to use either PID or INDI for the position control? Just trying to to think if this is actually that necessary like this, as this enables people to change controllers on the fly, which results in both controllers being made and that might be overkilled. Could this also be handled with a define or a make flag? So that at build people can decide if they want position INDI or PID?

The main reason why I introduced this variable is to differentiate between two main use cases I needed:

Attitude INDI only

Position and Attitude INDI

I can remove position PID from attitude INDI completely and at first structure the INDI controllers (into different files) as you suggested at the bottom. Thus, there will remain only two options:

Attitude INDI only

Position and Attitude INDI

which are controlled by the outerLoopActive.

Later, (if needed) one can add position PID.

Hmm yeah good point. Just leave as is for now. I don't think it is a big issue on a second thought and I actually would like to try out indi/pid with my position controller script that I just pushed to the python library.

knmcguire · 2020-03-31T13:48:51Z

src/modules/src/controller_indi.c


 static float r_roll;
 static float r_pitch;
 static float r_yaw;
 static float accelz;

+static vector_t refOuterINDI;		// Reference values from outer loop INDI
+static float outerLoopActive; 		// if 1, outer loop INDI is activated


OuterloopActive is a boolean right? You should define this as a boolean and initialize it as = false. (don't forget to change this at the parameters as well)

I've changed it as you suggested, however I am not sure if there is a boolean type for parameters (that's why I defined it as PARAM_UINT8).

Indeed, there is no boolean for parameters, but uint8 is just fine. Thanks for changing it

knmcguire · 2020-03-31T13:57:24Z

src/modules/src/controller_indi.c

-		rateDesired.yaw = yaw_kp*(attitudeDesired.yaw - state->attitude.yaw);
+		//rateDesired.yaw = yaw_kp*(attitudeDesired.yaw - state->attitude.yaw);
+		attYawError = attitudeDesired.yaw - state->attitude.yaw;		
+		if (attYawError > 180.0f) {


In PID controller file

crazyflie-firmware/src/modules/src/controller_pid.c

Line 73 in 1fdca2d

attitudeDesired.yaw = capAngle(attitudeDesired.yaw);

, a static function capangle is used. Maybe do it in the same way?

Anyway, if we get more controllers we probably need to think about making a separate file with controller helper functions, but that is something for the future.

knmcguire · 2020-03-31T14:01:02Z

src/modules/src/position_controller_indi.c

+}
+
+
+PARAM_GROUP_START(INDI_Outer)


Could you call the parameter group instead: posCtrlIndi, to keep it similar with

crazyflie-firmware/src/modules/src/position_controller_pid.c

Line 284 in 1fdca2d

PARAM_GROUP_START(posCtlPid)

Do you also have parameters for velocity? if not you can also just call it 'ctrlIndi'

I am not sure if I understand this. The PARAM/LOG groups of the position INDI do not only include the param/log variables for position and velocity, but also for acceleration, thrust, gains etc.. That's why I have put all these variables in one group that belongs to the position INDI, the same way it is done in the attitude INDI file. I renamed it from INDI_Outer to posCtrlIndi.

That is true, but we will think about the name change later. For now it is important that all the control parameters can be found at the same place as they are listed in alphabetical order in the parameter list in the cfclient.

Thanks for changing it

knmcguire · 2020-03-31T14:02:33Z

src/modules/src/position_controller_indi.c

+
+
+
+LOG_GROUP_START(INDI_Outer)


posCtrlIndi

Separate log group for velocity (velCtrlIndi)

Please see the answer to the previous comment.

knmcguire · 2020-03-31T14:11:17Z

src/modules/src/position_controller_indi.c

+	};
+
+	// Compute transformation matrix from body frame (index B) into NED frame (index O)
+	float M_OB[3][3] = {0};


In general I would prefer if matrix acceleration of the stm is used, so using the arm_matrix_instance_f32 and such like in kalman_core.c. But this is not a huge part of the code. Let us know if you want to change this or rather save this for another pull request

If possible, I would leave it for the next pull request.

Oke no problem

knmcguire · 2020-03-31T14:16:58Z

So I added some more comments. It is mostly about coherence with the rest of the firmware, to make sure it is similar to how the PID and Mellinger is implemented.

Also, since now there are 2 separate PID controllers, it might be good to separate the attitude INDI from the controller_indi.c in attitude_controller_indi.c. In this way it is similar to how it is handled for PID.c. So that you get the following structure of calling functions:

controller.c
- controller_indi.c
  - attitude_controller_indi.c
  - position_controller_indi.c

I hope that is a bit clear?

…ed, param/log group renamed

knmcguire · 2020-04-02T08:02:59Z

The changes are sufficient for now. Thank you for implementing them. I will try out the indi controller with outerloop on and off in the lighthouse system one of these days. If that is going well, I can merge this pull request :)

volmen3 · 2020-04-02T14:07:08Z

Cool! I forgot to mention something: the position (pos_set_x, pos_set_y, pos_set_z) is commanded in meters and is defined in the inertial frame. The inertial frame is initially (at start) aligned with the body-fixed frame of the Crazyflie as defined here: https://vignette.wikia.nocookie.net/gtae6343/images/0/00/Body_centered_frame.jpeg/revision/latest/scale-to-width-down/340?cb=20091015230621
So, to fly up you would command something like this: pos_set_z = -0.4, which will put the Crazyflie 40 cm above the floor.

knmcguire · 2020-04-02T15:38:42Z

Ah good you mentioned it. This will be a bit of a problem for compatibility... It would be best that as soon as you get the set point in your controller, you invert the y- and z-axis. The z-axis is defined to be upwards towards the sky in the crazyflie.

knmcguire · 2020-04-02T15:58:09Z

Ah I see. So you give the setpoints as a manually parameter and not through the setpoint_t struct as all the others. This is quite important, so please look at the pid controller as reference:

crazyflie-firmware/src/modules/src/position_controller_pid.c

Line 164 in f028e7d

    
           void positionController(float* thrust, attitude_t *attitude, setpoint_t *setpoint,

. The setpoints are given in global coordinates but the controller than transforms them into body fixed coordinates as you can see there.

Sorry to be strict on this, but it is necessary to use the same structure and manner as the other implemented controllers. That will keep the code manageable and will make it more future proof if more controllers are added. So this way, from the python script one can easily just choose an controller and give the set points and more people will use your controller.

Let me know if it this will be possible for you to change or if you need some help! I can also make some changes to the code.

volmen3 · 2020-04-05T09:35:48Z

I have noticed that the reference values are usually provided by the setpoint struct. Unfortunately, I could not found any clear documentation/instruction how to provide those setpoints and at the same time being able to plot current log values. That is the reason why I implemented reference values to be set through the parameter section. This way I was able to test the implemented algorithm. To use reference values from the setpoint struct one just needs to replace pos_set_x/y/z with the position values from the setpoint. That is why i wrote in my first post: "If the current handling of the control inputs does not correspond to some of the internal guidelines, feel free to adjust it", since you know better the overall software structure. :)

knmcguire · 2020-04-06T06:26:46Z

Ah ok. Since we would definitly want this in the code I will help you out. I will look at changing to code to be suitable for merging. In order to do that, I will need push access to this branch of your repo. Could you double check if I have so, you are able to select this in the pull request I think (https://help.github.com/en/github/collaborating-with-issues-and-pull-requests/allowing-changes-to-a-pull-request-branch-created-from-a-fork)

volmen3 · 2020-04-06T08:14:36Z

Thank you for the help! And yes, as I see, you should also be able to modify the code:

knmcguire · 2020-04-06T09:54:19Z

alright perfect. I will see what I can do this week. I'll update here

https://github.com/volmen3/crazyflie-firmware into volmen3-SA_INDI_outer_pos_pr

knmcguire · 2020-04-09T16:21:18Z

So just an update. I was able to verify that the parameter way indeed worked for the controller. I've implemented the setpoint way but it is not quite the behavior that I want so I haven't pushed it yet. So it still needs some more investigation, but that will be for next week:) happy easter!

volmen3 · 2020-04-09T18:40:44Z

Thank you for the update. Wish you happy holidays as well! :)

…ariables

knmcguire · 2020-04-15T18:49:24Z

Fwew that took a bit more work than expected ':) I looked over the part in controller_indi.c that disabled the use for setpoints (the passthrough of the setpoints were in a modeDisable if statement). But I tested it and it worked. The attitude controller tuning seems indeed better but the position tuning can be a bit better. Right now the PID controller is faster still. But at least it is all sufficient to merge it all now.

Some things that I would still want to see in the future is:

More use of existing vector/matrix structures of the crazyflie firmware
Fully separating position and attitude controller (attitude is still too much integrated into controller_indi.c)
Documentation to explain how the controller works in practice (https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/functional-areas/controllers/)

This is not necessary for this pull request, but I'll make an ticket/issue in github to remind ourselves in the future. In general I think that the entire controller structure in the crazyflie needs revamping. I've also been working on a controller tuning script in this repo (bitcraze/crazyflie-lib-python#146). Would be great if we could also add a tuner for the INDI eventually :)

Anyway, look at the changes I made and say if you are okay with it. I removed some variables that were not necessary.

volmen3 · 2020-04-17T08:37:12Z

Hey!

Thank you very much for helping with the modifications!

From my side all the changes are fine (especially if you think they suit well the overall handling of the reference values of the Crazyflie).

By the way, what is the exact procedure to control the Crazyflie through the setpoint struct on the example of this new INDI position controller? Should one use the position_commander_demo.py script for that, or could you please shortly describe how it is done? Beginning with the change of the controller type (since during my implementation I have changed the controller type directly in the stabilizer.c file :) ).

knmcguire · 2020-04-17T11:09:30Z

You can try the autonomousSequence.py in the crazyflie-lib-python repo in examples. In run_sequence you can set the controller with:

def run_sequence(scf, sequence):
    cf = scf.cf
    cf.param.set_value('stabilizer.controller', '3')
    cf.param.set_value('ctrlINDI.outerLoopActive', '1')
    time.sleep(1)

On the top of the script you can set the positions where the crazyflie will fly to. I set it to first (0,0,0.5,0) to make it just take off for a half a meter just to try it out.

volmen3 · 2020-04-17T11:58:25Z

Thanks for providing the example. I have tried it and it worked!

knmcguire · 2020-04-20T10:09:10Z

alright! I will squash and merge this pull request and make an issue for the wanted changes for the INDI controller :) Thanks again!

volmen3 · 2020-04-20T11:31:48Z

Thank you! :)

knmcguire · 2021-04-16T08:14:39Z

Hi @volmen3, we have some questions about the INDI controller on the forum. Could you take a look at it and answer if you have questions? Thanks!

https://forum.bitcraze.io/viewtopic.php?f=6&t=4786

knmcguire · 2021-05-20T07:42:58Z

Hi @volmen3. We are currently busy with adding descriptions params and logs variables, for which INDI we still need some help. Also we want to access how to setup controllers in the firmware in the future and would like to hear your experience with it as well. Could you contact us by email: contact_at_bitcraze.io so that we can include you in the discussions? Thanks!

ParamTab: UI fixes

volmen3 added 8 commits January 20, 2020 17:47

Implementation and some parameters (G1, G2, PD-Gains) corrected

3967fdf

PD gains changed

0683eb9

Defined some variables for logging testing

6b3a19a

Yaw problem solved

ab91748

PD gains changed

f219aec

refactoring

41f0584

Prepared for pull request

08f871e

Prepared for pull request 2

37543f8

knmcguire reviewed Mar 30, 2020

View reviewed changes

Changed controller type to default

4bb0b36

knmcguire reviewed Mar 31, 2020

View reviewed changes

volmen3 added 2 commits April 1, 2020 10:18

Makefile changed, matlab scripts added, type of outerLoopActive chang…

6c08682

…ed, param/log group renamed

Implemented capAngle()

4e91709

knmcguire added 2 commits April 9, 2020 14:09

Merge branch 'SA_INDI_outer_pos_pr' of

51ec669

https://github.com/volmen3/crazyflie-firmware into volmen3-SA_INDI_outer_pos_pr

added setpoints to position controller indi

48fbb78

fixed unresponsiveness due to modes and removed unnessary parameter v…

e517ab1

…ariables

knmcguire merged commit c4beeaf into bitcraze:master Apr 20, 2020

knmcguire added this to the next-release milestone Apr 29, 2020

cafeciaojoe pushed a commit to cafeciaojoe/crazyflie-firmware that referenced this pull request Sep 27, 2024

Merge pull request bitcraze#568 from bitcraze/jonasdn/560

aed3c27

ParamTab: UI fixes

		}


		PARAM_GROUP_START(INDI_Outer)




		LOG_GROUP_START(INDI_Outer)

Incremental Nonlinear Dynamic Inversion (INDI) position controller for the Crazyflie #568

Incremental Nonlinear Dynamic Inversion (INDI) position controller for the Crazyflie #568

Conversation

volmen3 commented Mar 29, 2020

knmcguire commented Mar 30, 2020

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

knmcguire Mar 31, 2020 • edited Loading

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

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

knmcguire commented Mar 31, 2020

knmcguire commented Apr 2, 2020

volmen3 commented Apr 2, 2020

knmcguire commented Apr 2, 2020

knmcguire commented Apr 2, 2020

volmen3 commented Apr 5, 2020

knmcguire commented Apr 6, 2020

volmen3 commented Apr 6, 2020

knmcguire commented Apr 6, 2020

knmcguire commented Apr 9, 2020

volmen3 commented Apr 9, 2020

knmcguire commented Apr 15, 2020

volmen3 commented Apr 17, 2020

knmcguire commented Apr 17, 2020

volmen3 commented Apr 17, 2020

knmcguire commented Apr 20, 2020

volmen3 commented Apr 20, 2020

knmcguire commented Apr 16, 2021

knmcguire commented May 20, 2021

knmcguire Mar 31, 2020 •

edited

Loading