INDI: Incremental Nonlinear Dynamic Inversion

[esaldiran]

This is an implementation of the Incremental Nonlinear Dynamic Inversion(INDI) controller I developed for my Master's degree.

Disclaimer: This PR is not intended to replace the current control architecture in any way. The technique used is relatively new and did not go through the rigorous testing and corner case analysis the current controller has. Its purpose is to provide an example for other researchers or enthusiastic to use and get inspired by and also to work out a more systematic and easy way to implement and test custom control algorithms.

What is this?

The INDI controller reduces the nonlinear dynamics of vehicles to the linear one by incorporating actuator feedback information through feedback linearization. tau_ref: the vector to be sent to the mixer after some scaling to match with ArduCopter mixer. tau_0: the torque vector calculated from the current motor rpm. alpha_0: the filtered angular acceleration v: the angular rates controller output. I_v: vehicle moment of inertia matrix

Basically, the INDI controller calculates the torque vehicle should have, which is mixer input(tau_ref), by comparing commanded angular acceleration(v) with the current angular acceleration(alpha_0) and adding their difference to the torque(tau_0) produced by the motors.

Without going into too many details, the INDI controllers have the following pros and cons. Pros:

• The reduction in vehicle dynamic allows for designing a simple attitude and position controller, using only P gain.
• INDI controller has build-in integrator behavior without using any integration.
• Robust to the change in vehicle dynamic for a fixed controller and filter gains.
• Better disturbance rejection capability that only depends on filter cutoff frequency.
• Fewer parameters are required, 7 related to vehicle dynamics and 15 related to controller and filter( I will drop two of them.)
• More intuitive tuning. Tracking and disturbance rejection capability can be adjusted with separate parameters.

Cons:

• Although it is robust to the change in the model parameters, it requires vehicle-specific parameters, namely the mass, a moment of inertia matrix, arm length, scaled throttle to motor speed coefficient, and propeller thrust and torque coefficient.
• It requires rpm feedback, which introduce additional complexity and dependency on the safety-critical component of autopilot.
• I only tested this with bdshot rpm feedback which practically provides loop rate rpm information, but I expect the controller performance to degrade under less frequent rpm updates.
• The INDI controller shines itself when the vehicle is at a high-speed forward flight regime, in which poorly tuned PID controller would have a lightly damped response. Given the limited operation regime where it performs better, I am not sure simpler and more intuitive tuning is enough to present as an argument.
• It is new and untested, although, in theory, it should provide better gust rejection and trajectory tracking. It is hard to tell without testing with the different vehicle types and under different flight regimes.
• No consideration is given for bumpless transfer between different flight modes or controllers. During testing this resulted in a lightly damped response for aggressively tuned position controller when flight modes switch to the GUIDED from a hover.

Flight Test Result

I tested this controller on 250 mm racer quadcopter last year under 3 different test scenarios.

Scenarios 1 and 2 were the trajectory tracking for circle and lemniscate-shaped trajectories. The blue and red lines represent the INDI and PID controller for both graph, respectively. In these tests, the position, velocity and acceleration target were feed to the both controller. ||r_ref-r||_2: horizontal distance error in m ||v||_2: north and east speed norm in m/s ||a||_2: acceleration vector norm in m/s^2 T_ref,tot: Throttle output (CTUN.ThO) The mean position error norm is reduced from 1.22 m to 0.32 m for circle trajectory and from 0.78 m to 0.21 m for lemniscate-shaped trajectory.

In Scenario 3, the vehicle is commanded to lean in the pitch axis and waited to reach a steady-state speed of approximately 30 m/s in ALT-HOLD mode. The goal of this experiment is to evaluate attitude tracking and attitude hold performance of controllers during the fast-forward flight. In this flight regime, the vehicle experience high moment drag. The inflow angle with respect to the propeller plane changes significantly, which changes propeller thrust and torque coefficient significantly. The attitude tracking result of PID and INDI controllers are given on the left and right respectively. The PID controller has an overshoot of 60 degrees and shows low damping oscillation at a speed between 30-15 m/s. After the airspeed decreases below 15 m/s, the PID controller displays a high damped closed-loop response.

All of these flight tests were conducted last year using STABILIZE, ALTHOLD, LOITER, and GUIDED mode. I rebased this implementation to the master and fixed all of the merge conflicts due to significant changes in attitude and position controller and lastly tested it in SITL to make sure it is functioning properly. During my initial implementation, I had to hack my way around to allow me to switch between PID and INDI controller in air.

How to play with it?

1. Start ArduCopter SITL with Matlab/Simulink backend. ./build/sitl/bin/arducopter "-M","json:127.0.0.1"
2. Start vehicle dynamic simulation in Matlab Open matlab and go into ../libraries/SITL/examples/JSON/MATLAB/Copter_Simulink directory open InitSimulation.m file and run it
3. Open mavproxy and assign RC channel to INDI_CH parameter. param set INDI_CH 7
4. Flip INDI_CH switch so that it is above 1700 to activate the controller. rc 7 1800 (For some reason this does not work in mavproxy, I had to go into libraries/AP_HAL_SITL/RCInput.cpp and change the _sitlState->pwm_input inside uint16_t RCInput::read(uint8_t ch) function)

Tuning

• The attitude and rate controller gain depends on the motor dynamic only. It should be changed in reverse order with the motor size. For smaller motors with high kv, higher gains are permissible.
• If the ratio between attitude and rate controller P gain is kept as 4, the controller should give a critically damped response. Ex: INDI_ANG_RLL_=7.5 then INDI_RAT_RLL_=30, visa versa.
• There shouldn't be need to change the position controller gains.
• Increase INDI_TRQEST_FILT to get better disturbance rejection. Think of this parameter as D term in the rate PID controller. Higher the cutoff, the noisier the output signal becomes. This parameter must be at the same cutoff frequency as the INS_ANG_ACC_FILT parameter. No matter how low or high the cutoff frequency gets these two parameters should be the same.
• There shouldn't be a need to change INDI_STHEST_FILT parameter. But make sure it is the same as the INS_ACCEL_FILTER. Similar to the torque estimation filter and angular acceleration filter, specific thrust and acceleration filter cutoff frequency must be the same.

Conclusion

I am aware this PR might not be merged in its current state. But I can work on it to allow easy and systematic implementation of a custom controller. I saw ADRC: Active Disturbance Rejection Control , but that implementation assumes we generate an output signal in the PID controller, which this implementation does not do.

This PR adds,

• Copter SITL Simulink model
• rpm field to the SITL_JSON backend
• method to read all motor rpm in AP_RPM
• angular acceleration variable in AP_InertialSensor
• method to share angular acceleration in AP_AHRS_View
• hacky way in AC_AttitudeControl and AC_PosControl library to switch between two controllers
• lua script examples
• INDI controller library

Acknowledgment

Firslty, I would like to thank @andyp1per and @bugobliterator for implementing bdshot protocol, it was the last bottleneck for me before the real flight tests. I also would like to thank @lthall and @rmackay9 for the attitude controller and mixing algorithm, I can't imagine flying at these extreme flight regime without all of the protection. I would like to thank @IamPete1 for Matlab SITL backend, it is order of magnitude faster and lightweight than my own implementation. Lastly, I would like to thank all of the ArduPilot team for all their effort.

TODOs:

• [ ] add SITL JSON backend related changes
• [ ] use AP_ESC_Telem for rpm feedback for both real and SITL vehicle
• [ ] add angular acceleration calculation related changes
• [ ] add better way to handle different controller in AC_AttitudeControl and AC_PosControl

[hendjoshsr71]

Very cool! We were just talking about researchers using ArduPilot and how we can do better in that area. I probably wont get to playing with it for a while but look forward to it.

Is your thesis complete or still being worked on? If it is complete attaching it here and linking to the publication at your school's library would be good to do.

One thing to think about is creating separate PRs for any commits that you think would be immediately useful to the greater community.? Less annoyance for rebasing in the future.

[IamPete1]

Would be great to get the simulink stuff in with the JOSN backend changes, looks like that could go easily.

[esaldiran]

@hendjoshsr71 I have seen on multiple occasions people asking how to implement their own controller and I believe there is something we can do to improve ArduPilot integration in the research area.

My thesis is finished, we turned it into a publication but it is still in the review process. I can email you a copy of the thesis if you are interested. I realized, that I only provided instructions about how to use it in SITL. If/When you are planning to test, please let me know, there are still a few things that need to be done to make it less restrictive on the vehicle setup(frame type, rpm source, etc.).

@IamPete1 I created a separate PR for SITL JSON backend changes. I would welcome your input.

[lthall]

Hi @esaldiran,

First up congratulations on finishing your thesis. Nothing like binding that up and saying "Finished"!

You have done a really nice job here and I really appreciate the effort you went into your PR explanation. Clear, very complete and well written!

This is a great example of what I was attempting to support with the structure of the control code. I hoped that I could create a structure that could easily be overwritten with alternative control systems, preferably using the C class structures and the override type calls. This could then be implemented as a compile time style choice, it does mean you can't bail out using the existing code or switch between controllers though. So maybe people with a better understanding of C++ may have better ideas. (I am probably the least capable dev in this area).

I would be happy to support changes or enhancements that would make it easer to integrate alternative control systems like this into various levels of the code in a sustainable and supportable way.

We would need to make some sort of decision that these secondary control systems would be

• removed if they stopped working,
• were only made available, not supported by the dev team. Support would need to be provided by the community of people using the secondary controller
• are experimental or use at your own risk.

But a well written control system, backed by a thesis for reference would be a good candidate to do this for the first time. A good example for others to follow also.

This would also open a path for these systems to take over as the primary control system or provide enhancements to our existing control system through example. At a minimum this would provide a way for people to more easily use ArduPilot as a control research or development platform.

[MichelleRos]

Fascinating to hear that it's in fast forward flight that its performance actually improves compared to PIDs. I'm very interested to test this controller once I have a quad flying again, as I've also noticed that it's once I'm flying faster that the PID controllers' performance becomes problematic.

Is this PR of yours ( https://github.com/ArduPilot/ardupilot/pull/21699 ) a reimplementation of this code under the Custom Controller framework?

[esaldiran]

Thank you for showing interest @MichelleRos. I was surprised to see such performance differences.

Is this PR of yours ( #21699 ) a reimplementation of this code under the Custom Controller framework?

Yes, I adapted this implementation to custom controller framework, but I lightly tested it in SITL. Currently, It only supports quad X frame type due to the way torque is estimated from RPM measurement. You can change it to different quad motor order by changing order and sign in control_alloc_G1 variable.

Current gains works okay for a F250 racer frame. Tag me if you need help with tuning.

[MichelleRos]

@esaldiran Thank you for your reply. I am planning on also building a 250 mm quad for my next vehicle, so your gains should work really well.

I'm curious about what the full flight performance was like between the two controllers. Any chance you could send me the logs from flying them? I'm also on Discord under the same username if that's easier.

[esaldiran]

@MichelleRos I had to dig through my archive but found the flight logs. I tried to give some info to make it easier to check.

The numbers following circle or lemniscate are the speed and radius of trajectory. The trajectories are commanded with Lua script. Legacy refers to default PID controller.

RCIN CH7 enable(high) or disable(low) Attitude Controller Feed Forward parameter. ffon or ffoff at the end of file name refers to this parameter. RCIN CH8 activates INDI controller when high.

You can check attitude and position tracking using ATT and PSC log regardless of which is controller used. I am sorry log names are not self explanatory but you can find more details in AC_INDI_Control::write_log function. IND1 logs target and measured position, velocity IND2 logs target, desired and filtered linear acceleration IND3 logs target and measured attitude, angular velocity IND4 logs target, desired and filtered angular acceleration IND5 logs target and estimated specific thrust, torque

Logs ├───210601_QAV250_Pixracer │ ├───01_circle_10mps_10m_indi_legacy_attsatur │ ├───02_circle_15mps_15m_indi │ ├───03_circle_10mps_15m_indi_legacy │ ├───04_circle_7mps_15m_indi_legacy │ ├───05_circle_4mps_15m_indi_legacy │ ├───06_lemniscate_95cw_314cm_indi_legacy_attsatur │ ├───07_lemniscate_35cw_1500cm_indi_legacy │ ├───08_indi_legacy_70lean │ └───09_indi_legacy_60lean ├───210607_QAV250_Pixracer │ ├───01_doublet_roll_30deg_2s_indi_legacty_ffon │ ├───02_doublet_roll_30deg_2s_indi_legacty_ffoff │ ├───03_doublet_pos_1m_2s_indi_legacty_ffoff │ ├───04_doublet_pos_5m_2s_indi_legacty_ffon │ ├───05_pos_hold_indi │ └───06_pos_hold_legacy └───210609_QAV250_Pixracer ├───01_pos_hold_indi └───02_pos_hold_legacy

[MichelleRos]

@esaldiran Thank you. I'll have a look.

[aogrcs]

I test this for master branch and matlab 2022b using wsl, and it says that not correctly using InitSimulation

[esaldiran]

I test this for master branch and matlab 2022b using wsl, and it says that not correctly using InitSimulation

@aogrcs This probably needs rework to get it working with master. if you are trying to compile INDI controller for flight testing, there is a similar implementation utilising the custom controller library in https://github.com/ArduPilot/ardupilot/pull/21699. That PR doesn't have position controller stuff like this PR. But it shouldn't be too hard migrate position controller functions to there. You can ping me on discord on research channel if you need help.

[aogrcs]

I test this for master branch and matlab 2022b using wsl, and it says that not correctly using InitSimulation

@aogrcs This probably needs rework to get it working with master. if you are trying to compile INDI controller for flight testing, there is a similar implementation utilising the custom controller library in #21699. That PR doesn't have position controller stuff like this PR. But it shouldn't be too hard migrate position controller functions to there. You can ping me on discord on research channel if you need help.

Hi, there is some problem for discord in China, when it was good, I would ping you, thanks