# Mathematical modelling

I didn’t believe mathematical modelling was necessary / possible for a real-time system such as a quadcopter, so my approach has very much an iterative

• come up with a theory
• test & tweak it until it works.

One of the guys who has commented here, Oscar, has a lot more mathematical approach on his blog, and one piece caught my eye today: using an audio spectrometer to measure the spin rate of the blades and correlate that to motor voltage and ESC PWM.

Ultimately the blade spin rate doesn’t matter to me – the PIDs set the spin rate, and sensor feedback makes sure it does what the PID target asks of it. But it’s interesting to understand mathematical model too even if it’s not used.

In theory, the blade spin rate is proportional to the voltage applied (apparently).  Using an audio spectrum analyser (many smart phone apps are available that do this), it should be possible to spot peaks in the audio spectrum corresponding to the blades spinning.  There should be a very tight cluster of 4 for a quadcopter.  That’ll give a nice check that all the blades are spinning at the same rate when they should be.  It should also allow double checking the PWM rate (I think). I am speculating also that if I have a duff motor causing the yaw (my only theory I have whose proof has eluded me so far), it should stand out like a sore thumb in the audio spectrum.

Motors come with a Kv gain which maps between RPM and applied voltage:

rpm = Kv * V

rpm will match the frequency of the peaks in the audio spectrum from the analyser.

The ESCs will always put out full battery voltage, but only in pulses (hopefully those fed in via the PWM), such that a 1.5ms PWM pulse fed to the ESC => 50% battery power applied => ~ 5.5V (0.5 * 11.1V from my 3S LiPo) => 5400 rpm (Kv = 980 for my Tiger Motors) => 90Hz audio spectrum peak.

Given that tomorrow’s weather is poor, this sounds like an interesting little side experiment to play with. And it might just show that one of my motors is indeed duff and so prove itself useful too!

I should mention that there’s a lot more interesting stuff on Oscar‘s blog covering the math(s) and his progress – certainly worth a read if you want something more substantial than my mindless witterings!

In fact I think I’ll be heading back because there’s well defined equations for blade measurements (length / pitch) compared to power / rpm, and together that ties PWM frequency to power (= acceleration) applied.  I just love it when things slot into place like this.

# I used to be indecisive…

but now I’m not so sure…

whether I actually need a horizontal acceleration PID.  I’d considered adding the horizontal acceleration PID between the horizontal speed PID and the pitch / roll angle PIDs.  Its output would set the angle target for those PIDs, but I’ve come to the conclusion that there is a direct mathematical link between horizontal acceleration (the output from the horizontal speed PID) and the tilt angle target set for the angle PIDs

Consider we want 1g acceleration horizontally, and 1g acceleration vertically to give us horizontal acceleration – i.e. no rise or fall in flight height.  Then the power must be applied equally horizontally and vertically, meaning a 45º tilt.  Or putting it slightly clearer, the acceleration from the accelerometer is measured in g’s, so if we setup the gains of the horizontal speed PID to output in (approximate) g’s, and denote earth axis acceleration along the x and z axes as aeax and aeaz respectively then

tan(θ) = aeax / aeaz

But for horizontal flight aeaz = 1g. So

θ = atan(aeax)

This means the target acceleration from the horizontal speed PID output (i.e. target acceleration) if measured in units of g maps directly to a target angle for the pitch / roll angle PIDs which is “a good thing”TM as it’s much better than adding a noisy accelerometer PID into the system.

# Trampoline testing and virtual reality

With a little thought, I have a better idea of what’s required from the trampoline. Consider the following:

The top ‘picture’ shows flight – plotting height over time. The quad sits on the ground, then rises at fixed speed for a bit, hovers for a bit, descends for a bit. and sits back on the ground again – this is the external set of targets for the quad – what the human wants it to do.

The middle ‘picture’ shows the same, but plotting vertical speed over time. The sitting quad starts with 0 speed; a pulse of linear speed integrates over time to produce the sloped rise in height in the first picture. Descent is clearly the opposite. This is what the outer PID does – it takes the target set by the first picture to produce and output to achieve the desired vertical speed

The final ‘picture’ shows the same but from the acceleration point of view. Violent spikes of power are applied to the drone to change it speed as per picture two; momentum maintains that power to change it’s height as per picture one. This is what the inner PID does – it takes the output of the output PID as it’s target acceleration, and combined that with the gyro inputs to control the acceleration.

OK, in the real world, all the sharp corners will be rounded or wobbly depending on how well the PIDs are tuned. But the sharp diagrams provide excellent PID ‘targets’ for simulation – and that’s the next direction I’m going: using these targets, and some basic math(s) and a spreadsheet, it should be possible to tune the PID in simulation alone except for one missing link, and that’s where the trampoline steps in: I need a mapping between the blade PWM inputs, and the net acceleration that produces. That simply means that I do several runs at various PWM inputs, and use the accelerometer to measure the resultant acceleration.

Don’t think I’ll need it, but the drone currently weighs 1509g (1.5kg) currently:

That’s with the new case and corresponding diet Model A RPi, micro-USB adapter, carbon legs and the golf ball feet!

# Outdoor test flight #2…

has been delayed by bad weather!

The forecast here in the Cotswolds, although sunny shows 13 – 16 mph winds until at least Thursday.  To handle the winds requires exactly the opposite logic required to make the drone move forwards, backwards, left or right, and currently I’ve not implemented that as I know it’s complex…

Take for example trying to move forwards at say 10cm/s…

• to get forward power requires the drone to lean forwards at some angle, call it theta, which comes direct from the integrated gyro sensors
• however, on tilting the drone from a horizontal hover, it’ll start to descend as the blades are no longer blowing straight down
• which means upping the power to all blades by a fixed amount to keep it level
• but to detect it’s level uses the accelerometer which is no longer horizontal, but leaning at angle theta, so that angle needs to be compensated for.
• and finally, what we need to achieve is a PID, fed by all this information, used to ensure that we are in a moving forwards at a stable 10cm/s

So the simple target of 10cm/s forwards requires the gyro PID fed with a target from an integrated and lean compenstated accelerometer forward PID.  But at the same time, the z axis acclerometer must be used to maintain hoizontal flight.  But in doing so, that will affect the speed the drone flies horizontally. And the lean compensation comes from the integrated gyro sensor.  So there’s some funky math(s) to be applied here, using Euler angles possibly, combined with accelerometer PIDs for the x and y axes to attain this.

Hmm, sounds tricky!

Which leaves me with 2 options for these initial flights – wait for the wind to stop, or test indoors.  I think I’ll go with the latter.

# What’s all the fuss about?

OK, so my past few posts are about solutions to a problem I’ve not described or shown evidence for. So here you are:

• Blue is the output from the accelerometer z-axis – nicely hovering around 0 as expected.
• Yellow is the summation of this within the code, to turn the accelerometer g-force into meters per second rise / fall
• Maroon is the same summation, but done in Excel based upon the Blue accelerometer outputs.

Maroon and Yellow should look very similar – do they to you? Me neither! The only difference is the stats (vert sim / maroon) are dumped every 0.1s but the real speed is calculated every 0.015s. Having spent a day going nuts trying to find the bug in the code that is causing this problem (as the most likely cause) and failing, I now have to consider the fact that there’s an awful lot going on between the stats snapshots, and perhaps I need to do something about it. First job though is to get some better stats for the gaps.

# Complementary vs. Kalman

Still digging and found this filter that I can understand and implement, unlike the Kalman which makes my brain hurt. Dunno whether it’s good enough, at the moment just another note to consider.

# Karma karma kalma kalma Kalman chameleon…

you pitch and roll, you pitch and roll. (with apologies to Boy George for mauling his lyrics)

While pitch / roll / yaw stability is good enough for the moment, I am having problems with vertical velocity, and that got me researching again. I don’t know yet whether what I’ve found is relevant to my problem, but it certainly is to longer term pitch / roll / yaw stability ; I’m currently summing the gyro outputs over time to come up with the resultant angles, and to be honest, they’re pretty good, but I wouldn’t want to rely on them for long flights – and this is what I stumbled on – Kalman filters. Not sure yet what to do with it, but I thought it worth sharing (and it means I don’t have to write a reminder down on paper!)

# Cutting corners =

Rounding Errors!

The rounding errors arose when converting from floating point to integers. For example

Float 3.4 => Int 3
Float 3.5 => Int 3
Float -3.4 => Int -3
Float -3.5 = Int -3

So you add 0.5 to solve these rounding errors, but just +0.5 won’t do. If you do that, you get

Float 3.4 + 0.5 = 3.9 => 3
Float 3.5 + 0.5 = 4.0 => 4
Float -3.4 + 0.5 = -2.9 => -2
Float -3.5 + 0.5 = 3.0 => -3

So for negative numbers you need to subtract 0.5 to ensure correct rounding:

Float 3.4 + 0.5 = 3.9 => 3
Float 3.5 + 0.5 = 4.0 => 4
Float -3.4 – 0.5 = -3.9 => -3
Float -3.5 – 0.5 = -4.0 => -4

To do this I ended up using math.copysign() to ensure the 0.5 ‘added’ had the same sign as the number it was added to.

I thought I’d got this sorted a couple of weeks ago, but in the past few days I’ve found a more subtle class related to the calibration. This entails taking 100 samples from the gyro and accelerometer over 10s on a level surface. The result is used for any live read-outs from the sensors when the drone is in flight. But I’d made the mistake of converting the offsets to ints, thus making the offset correction also slightly out.

So the net result is that other than the direct inputs from the sensors and the direct outputs to the PWM, everything else are now floating point to ensure accuracy in all calculations.

P.S. I should explain that I’m not completely inept, but I normally program in “C” which has strict types, which means it’s much easier to not write these bugs in the first place, and if you do, the compiler will spot the error or warn you. However interpreted Python makes it’s best effort to hide typing problems, which makes it much easier to make type casting errors, as I’ve demonstrated so well!

# Logging stopped the drone headstand

So I think I’ve found and fixed the cause of the drone headstand.

“You think you’ve found it”, I hear you say? Yes, well, this is through simulation as I don’t want to turn on the motors again without understanding what went wrong.

“Who cares, cuts to the chase!”

The problem is (I suspect) that when the drone tilts by a small amount, the z axis accelerometer starts reading less than the 1g down force set as its PID target on the vertical axis. That’s because the vertical axis and the z accelerometer axis are no longer aligned. So the z PID which takes the z accelerometer as it’s input ups the power to all blades to meet the current vertical axis target. That sets the drone scootling across the floor at an angle and the first impact then triggers the flip into a headstand.

The cause is that the target for the z PID is 1.0g absolute vertical, whereas the input to the PID is < 1g but critically that accelerometer is no longer vertical. But if I use the math(s) I’d found earlier, I have now “normalized” the z-accelerometer output to vertical and am using that as the z input. That was the main cause of the problem.

The noise I mentioned previously is probably not caused by the motors as the accelerometers do fluctuate even when the motors aren’t engaged. That’s not really a problem as long as the PID accounts for it. In this cause, the Z axis PID has too much emphasis on its proportional component (the P in PID) in comparison with the integral component (the I in PID). On the vertical axis, it’s the I component which needs to dominate, since it’s that which will power takeoff and vertical stability at a given height. The X and Y PIDs are the ones handling front / back /left / right tilt balancing.

Having realized that, my simulations are showing much more stable performance, and the few niggles I have are due (I hope) down to flawed simulation, rather than flawed PIDs.

I really am running out of excuses to not just set this thing loose!

“Well get on with it then!”

Not until I can have the largest, tallest room of the house emptied to provide a safe take-off site.

“What’s that room used for normally?”

The kids playroom! Think I’m going to have to take another off work while the kids are at school / nursery!

P.S. Processing the simulation results was greatly improved by adding a second handler to my logging. Now each diagnostic log can be written both the file and console. All that’s written to file are diagnostic stats, which then are fed into Excel LibreOffice spreadsheet to churn out some graphs. Compare and contrast the starting, and ending position of these this test. Notice the chaos ending in flat-lining on the before run? (The four lines on each graph represent the power (in PWM ratio units) applied to each motor).

# Turtle Pi Math(s)

So I’ve got a Turtle under my command, but the commands are all in steps of the motor – such as “move fwd 100” or “turn acw 37”.  I’d prefer the units to be meaningful approximations to reasonal units like millimeters or degrees, so that needs some basic maths.

The distance between the wheels is 160mm – let’s call it D.  The diameter of the wheel is 82mm – let’s call it d.  The motors performs 1 full rotation (360 degrees) in 200 steps.

So forward and backward are relatively simple math(s): distance travelled in millimeters, m = (n x π x d) / 200.  So to travel m millimeters, we need n = m x 200 / (π x d) steps.

Turning on it’s central axis is slightly trickier: n steps travel a distance of (n x π x d) / 200 as above.  But now this represents a partial arc of the circle the turtle is turning in – lets call that angle θ (theta).  So (θ x π x D) / 360 = (n  x π x d) / 200.  Cancelling the π’s leads to (θ x D) / 360 = (n x d) / 200.  Or for a given angle to turn, the number of steps is (θ x D x 200 / (d x 360)).

Finally, I also have a “sweep” action where one wheel is locked, and only the other moves. This effectively doubles D as now the radius of the circle is D rather than the diameter.  So the steps swept to move through an angle theta is now (θ x 2 x D x 200 / (d x 360)).

Having got the number of steps, all that’s left to do is to convert them from floating point to integers allowing for rounding errors.  I’ll post the updated code containing these changes, and changes related to the next post…