HefnyCopter2 implements full PID terms that is P, I & D. It uses the following approach.
My vision is to keep math as simple as possible and use superposition to accumulate different behaviors together based on my suggested framework.
Gyro values for balancing are in purple.
Accelerometer and accumulated Gyro are used together as a virtual angle device to get estimated Angle and implements the restoring function using a PID and are shown in green.
AccZ implements Positioning-Balancing and is shown in orange.
Sonar implements Positioning-Restoring and is shown in Red.
Please let me know how do you find this approach.