Control Practice

To do this control loop, we will have a function that is passed in the commanded value of the state and the current value. It will maintain two Kalman filters, one for the commanded value and one for the state error. The one for the control value is to calculate the rate of change of the commanded value, and the one for the state error will track the current error as well as its integral and derivative. It will also have a vector of Kp, Ki, and Kd, the proportionality coefficients for all the control terms.

With the command Kalman filter, the captain is free to suddenly change the command in a step manner from one value to another. Imagine what would happen in the landing routine when the commaded pitch is set to zero and the commanded altitude is suddenly set from five feet to zero. The proportional error would become very large instantly, and the controller would likely cut the power completely, leaving the aircraft to fall out of the sky. The controller plans to correct this once the vehicle has passed through zero altitude, but I think that the parking lot may have something to say about that as well.

With the Kalman filter on the commanded altitude, the command received by the control loop is not allowed to change instantly. The command will change smoothly, allowing the controller time to track it, and the derivative term will not act on the rate of change of the state variable, but the error between that and the commanded rate.

There are two Kalman filters because they are independent, and we don't want to pay the n^3 penalty. We will use the extended form of the velocity filter, because we already have code to drive that. For the state variable error, we will use the following state:

x=<P,I,D>


with a physics function

F(x)=<D,P,0>

and a predicted observation function

g(x)=P

with the actual observation as actual-commanded. The other is just a plain Kalman velocity driven by the commanded value.

So, the control function is handed a commanded value, an actual value, runs these Kalman filters, then figures the control value u=Kp*P+Ki*I+Kd*D. This control value can then be used to drive the motors. The altitude control loop generates a main rotor average throttle Ma, and the yaw loop generates a value Md. The tail rotor generates Mt. The super-controller which calls the control functions then commands M1=Ma+Md, M2=Ma-Md, and MT=Mt.

If it's so simple, why haven't I implemented it yet?