Yukari 4 and nowcasting

I have been convinced to do Yukari 4, by of all people, my lovely wife Kellie. I have a theory about what is wrong, and how to fix it with one wire and a software change.

Heading Reference

The hardest thing is getting an accurate heading reference. There are at least 4 possible sources:

1. Magnetic compass - We are carrying one, but it is close to the motor. I'll have to check some of the data to see if the motors influence it a lot.
2. Gyroscope - Subject to random walk drift and needs an accurate starting heading
3. GPS heading - The GPS calculates a heading itself and reports it in RMC
4. GPS delta-position - Use trigonometry and the position 1 second ago

 A proper solution would use a Kalman filter. I haven't gotten that to work on the robot yet. So, I am using primarily 2, with 3 to initialize and 4 to correct when in motion. It goes like this:

• We initialize the gyro using "Average-G". This is the average of the last 50 gyro readings on each axis. This average is subtracted from the in-motion readings to produce a zero-biased rotation rate
• We use the first RMC sentence to get our initial heading
• Once in motion, we use the gyro as the primary heading reference. We set up an inertial frame parallel to the initial orientation of the robot, and an initial identity quaternion. We use normal quaternion integration to integrate the gyro readings and get a new gyro-based orientation. We use that orientation to transform the vector pointing forward (the nose vector) from the body to the reference frame. The gyro heading is then the angle between the initial nose vector (same as the body nose vector) and the current nose vector. We use the initial heading to calculate the offset between the gyro heading and true north, and add this offset to the gyro heading to get the instantaneous heading relative to true north
• Whenever a large maneuver is executed, IE a large yaw rate is sensed, a countdown is set to 400 readings (4 seconds). When this countdown expires, we use method 4 to get the heading if the speed of the vehicle is high enough. We use this heading to calculate a new heading offset.

Improvements are as follows:

• We know the initial heading from the first two waypoints. Provided the robot is accurately aimed toward the waypoint, it will be plenty good enough.
• We are going to add a green button. Before the button is pushed, the robot will continually be recording the gyro readings, but will remember the last 50. Each reading, we will pop one reading out of the queue, and average it with the running total. The weighting will be 1 point for the new reading and 49 for the old average. In effect this takes the average of all the measurements from robot reset, but each old measurement has an exponentially decaying weight. We include some code to wait for 49 measurements before using a weighting of 49. We remember 50 measurements because the very act of pushing the green button will cause the robot to shake and will ruin the current measurements.

With these improvements,  I expect the initial heading to be much better and I expect to be able to run almost entirely on gyro readings.

Control Implementation

1) Do a bunch of refactoring to get the Kalman filter nice and portable.
2) Write a control loop structure which holds two Kalman filters and the Kp, Ki, and Kd.
3) Ignore the Kalman stuff to begin with. Just do a P controller on yaw.
4) Integrate the motor control stuff into Arisa.
5) Put Kyoko on the engine room board
6) Test the motor control stuff open-loop.

After quite some effort, I finally got Arisa and Kyoko talking to each other again. I had to cut all the traces on the engine room board for the SPI bus, turn I2C down to 200kHz (it's not really running that fast anyway) and put a couple of software patches into both programs, but Kyoko is now spinning the tail rotor on command from Arisa.

The bad news is that it is only spinning main rotor A. I think I probably damaged the connection to main rotor B when I was cutting traces. If this is it, I am going to build another engine room board. I am also planning on building a safety circuit to cut power to the engine room without an explicit command from the Arduino.

But, first thing's first. Get rotor B spinning, and the yaw control loop working. AVC looms.

Update: Good news! It is fixable in software. The problem is that the Arduino doesn't respond properly to two consecutive commands. So, we will just put all the motor commands into one packet.

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?

Control Theory

Control is the helmsman on this bridge. The captain (guidance) gives the order, and the navigator (navigation, all the Kalman filter stuff done up to this point) gives the captain the data needed for his decision, and the helmsman what he needs in order to control. Let's see if we can take this crew analogy right up to the edge. The helmsman and captain are blind, and as the navigator figures things out, he reads them aloud so the helmsman and captain can hear. The helmsman cares mostly about the current heading, which he gets when the navigator reads the compass. He also cares about the desired heading, which he gets from the captain. The captain cares about all the navigation stuff, such as latitude and longitude, as well as current heading, and he figures out the desired heading and tells the helmsman. Is that too far?

Error
Anyway, this control loop is given by navigation a current state element, and by guidance a desired value for that state element. The control loop then figures out the correct setting for the control that acts on that state element. To make it concrete, let's think about pitch. The guidance program knows that to maintain a certain speed, a certain pitch is required. It calculates that pitch (actually the sine of that pitch) and passes it as a commanded value to the control loop. The navigation program knows the current quaternion. It transforms a vector pointing towards the nose in the body frame into the inertial frame. The vertical component of this vector is the sine of the pitch. It will be positive if the nose is up, and negative conversely. This is the actual value. The job of the control system is to make the commanded and actual values match. When the values match, the error value is zero. When they are different, the error value is the difference between the two. So, to make them match, the control system tries to drive the error to zero.


Stability
Some systems are naturally stable. The classic stable system is a marble in a bowl. "An airplane wants to fly." Left to itself, it will largely fly straight and level. Any small disturbance like a gust of wind may knock the plane off of straight and level, but it will naturally correct itself. The aerodynamics designers work long and hard to give an airframe this stability, but once its there, the plane is stable largely without any effort by the pilot, who is then free to navigate and guide the plane. The airframe itself acts as the control system.

Some systems are unstable. The classic unstable system is a marble on a dome or a pencil balanced on its point. Any small disturbance in this system will be amplified, with the system falling farther and farther off balance. The Wright Flyer I was unstable, intentionally, and required a well-trained pilot as the control system to keep it nose into-the-wind. Anyone who has balanced a broomstick on end, or ridden a bicycle, has been a control system for an unstable system.

Some systems are neutrally stable. The classic example is a marble on a flat table top. If it is disturbed, it will not depart from its original condition, but will not return either. Steering in a car is neutrally stable.

One way to think about a control system is as a modification to a system to make it stable, and in particular stable around the desired value. You can think of it this way, that the control system is like a bunch of springs attached to the marble surrounding the desired point. These springs are strong enough to push the marble back to the desired point and overwhelm any instability in the system. The new system is stable, and stable at the point you choose.

The PID controller

The helicopter I have is stable in roll, neutrally stable in yaw and altitude, and unstable in pitch. There is no control for roll, and since it is stable in roll, none is necessary. The system will then run three independent control loops for main rotor average thrust to control altitude, main rotor differential torque to control yaw and tail rotor thrust to control pitch. These three variables are largely decoupled, and so can be handled independently.

The controller will be a PID controller, standing for Proportional, Integral, and Derivative. The control loop will look at the difference between the actual and commanded state, and generate a control signal proportional to the error. It will look at the total integrated error over the past, and generate a control signal proportional to this integral. It will look at the current rate of change of the error, and generate a signal proportional to this differential. It will then add them all up and use that control signal to drive the correct control.

The proportional part is obvious. If there is an error, you want to correct it, and drive the error back to zero. So, the proportional part is just the current value of the error multiplied by a coefficient. If the error is positive, you generally want to push negative, to push the error down, and conversely for negative error, so this proportional coefficient is negative.

The derivative part is there to keep the system from oscillating. If you have ever worked with differential equations, you have probably learned all about simple and damped oscillators. A simple oscillator is like a weight on a spring, or a pendulum. The restoring force is always proportional to the distance from equilibrium, so it is like the proportional control term. However, a system with just a proportional term will just keep bouncing around, always oscillating around the desired point with the same amplitude and frequency forever. To prevent this, we have a damping term. We drive the control system opposite to the direction of motion of the system, proportional to the speed. With enough of this term, any oscillations, either natural or driven by the proportional system, are damped out to zero.

The integral part is there for two reasons. The first is this: The proportional and derivative terms are somewhat at crossed purposes. The proportional part is trying to move the system back to the control value, while the derivative part is trying to stop the system completely. These two parts could balance out, leaving the system in equilibrium but not at the chosen point. The integral term is proportional to the integrated error in the system. Even if the error is constant, because the proportional and derivative terms have balanced out, the integral of the error will get larger and larger. So we attach a control to this integral error, proportional and opposite as usual.  This way, a constant error will lead to a larger and larger integral term, which will eventually break the deadlock between P and D

Secondly, if a system is not just unstable but actively pushed, a pure proportional controller will not be able to drive the system to zero error. Imagine a ball on an inclined plane. There is a disturbing force proportional to gravity on the ball. When the ball is in position, the P and D terms are zero, there is no control force, and the disturbing force pushes the ball away. The system will come to rest when the proportional term and disturbing term are equal, but this can only happen when the error is nonzero, since the proportional term is proportional to the error. The system will come to rest at a nonzero error. Wikipedia calls this phenomenon droop. Droop is bad.

In both cases, the integral term exists to destroy nonzero equilibrium error. It will grow larger and larger as needed. It can't be bargained with. It can't be reasoned with. It doesn't feel pity, or remorse, or fear. And it absolutely will not stop, ever, until the error is dead.

In our case, the pitch is unstable, because the center of gravity of the vehicle is not under the main rotors. If it were, pitch would be neutrally stable. This could in principle be achieved by shifting the components, or by adding ballast weight. But it would have to be totally accurate, and I don't think I can achieve that. So, PID it is.