0%
0%

# Adventures in state-space control, Arduino edition

Rudimentary, low-cost ball balancing table to investigate the joys of state space control

Similar projects worth following
1.5k views
Minimal (in the sense of cost and time) setup to explore state space control.

I've been interested in control systems for a long time. The convergence of math, electromechanics, sensors, and programming holds a place dear to my heart. Last year, I built this low budget ball balancing platform with my kids. The ball position is sensed using a resistive touch screen picked up for \$1.50 at AllElectronics. The platform is tilted using micro-sized hobby servos, \$2 each. Add an Arduino for a few bucks, a steel ball, wires and a battery holder and the total bill of materials was under \$20 and came together in an afternoon.

It initially started off using PID (proportional, integral, derivative) control using the Arduino and it worked reasonably well after finding the correct tuning parameters. I had ideas of converting to state space control-- using linear algebra to control stuff. It then sat on a shelf for a while until I saw an article on HaD titled, "Talking Head Teaches LaPlace Transform."

Steve Brunton has a bunch of videos on Youtube including a series called "Control Bootcamp." There are plenty of videos about control theory but precious few showing actual implentation. I wanted to see if I could actually get this to work.

How hard could it be?

• Measure the dynamics of the ball balancing platform.
• Come up with a set of equations describing the dynamics of the system.
• Convert to state space matrix form
• Convert continuous time system to discrete time system
• Simulate using the python control library to confirm correct dynamics
• Design an observer
• Design a feedback system
• Simulate some more to confirm it works as expected
• Convert above simulation to Arduino
• Tune parameters
• Tune the model
• Iterate above a lot
• Profit!

I found the entire process in turns daunting and not so bad. I would like to share some of the things I learned along the way. The python control library (https://python-control.readthedocs.io/en/0.8.3/) was very helpful. It provided almost all of the functionality of Matlab for this kind of work.

I think I bit off a little too much the first time around. I tried to use a Kalman filter as an observer because the resistive touchscreen sometimes glitches (perhaps from small dead spots in the resistive layer). I also tried to use a linear quadratic regulator for the full linear quadratic gaussian (LQG) enchilada. I want to go back to these at some point.

I had tantalizing hints of success. After a lot of parameter tuning and more trial and error than I care to admit, it worked-- sort of. I had naively believed that the magic of state space would take care of steady state errors. The steady state error of plain vanilla state space control is horrible-- i.e. any deviation from the ideal dynamics described in your equations will lead to steady state error. But the magic of state space control has solutions for this in the form of robustness methods.

You can see the platform in action here:

### Ball_Balance_v0.5.ino

Arduino code

ino - 4.41 kB - 10/13/2020 at 18:52

Adobe Portable Document Format - 253.10 kB - 10/13/2020 at 18:22

### 4. Kalman observer.ipynb

ipynb - 690.44 kB - 10/13/2020 at 18:18

### 4. Kalman observer.pdf

Adobe Portable Document Format - 514.04 kB - 10/13/2020 at 18:18

### 3. Robustness.ipynb

Jupyter notebook incorporating integrator

ipynb - 426.15 kB - 10/13/2020 at 18:17

• 1
1. System Dynamics of the Ball Balancing Platform

Fortunately the dynamics of the table are not too complicated. Let the state, x, of the table include 2 variables, position and velocity, x and v.

Given the relationship dx/dt = v, the state matrix equation is:

The only way to control the position is by changing the tilt of the table. We need to find the value of C such that it incorporates the relationship between the control input, u, the degree of tilt in the table, and the resulting acceleration on the ball. This relationship, including the small angle approximation of tilt angle (a = sin(theta) ≈ theta) and conversion to digitizer units, is all incorporated into the constant C.

In order to find C, the platform was tilted one unit to the right and the ball was rolled several times toward the left.

The timestamps and ball coordinates were sent to the serial port:

```### Serial capture
### time (ms), x and y position (digitizer units)
102, 183, -60
201, 150, -63
302, 120, -65
402, 95, -65
502, 74, -65
602, 58, -66
702, 45, -66
802, 35, -65
902, 31, -66
1002, 30, -68
.... lots more lines```

Which when plotted look like this:

Each of the black arrows point toward a ballistic trajectory, accelerating toward the positive x direction. You can see the occasional glitch in the position reading, likely due to dead spots in the transparent TIO (tin indium oxide) layer(s). One of these trajectories was chosen to fit the parameter C.

The red dots show the measured ball positions on the tilted table. The continuous lines show simulated step responses for various values of C.

The open loop system step response was simulated using the python control library control.step_response() function. As expected, when the table is tilted rightward, the ball accelerates to the right with a constant acceleration. And keeps going until it falls off the table.

It was not possible to get a perfect fit with the simplified model. The values shown above are for C= 335, 350, and 425 for the orange, blue, and green respectively. The blue, C=350, was chosen as a compromise. Perhaps the green would have been a better choice because it gives better short term approximations?

Convert to discrete time matrices using c2d( ) so that new states can be computed using

This gives us the following discrete time system:

```A = [[1.  0.1]
[0.  1. ]]
B = [[ 1.75]
[35.  ]]
C = [[1. 0.]]
D = [[0.]]
dt = 0.1```

The Arduino is capable of ball measurements, the feedback calculations, and updating commands to the servos in about 5 ms which would allow a maximum update frequency of 200 Hz. Given that servos update at ~50Hz, it probably doesn't make sense to go all the way to 200 Hz.

I chose a relatively low Tstep value of 0.1 s, or 10 Hz update frequency with the idea that if you can't do it right slowly, doing it faster won't help. Get it working first, speed things up later.

I've glossed over some of the implementation details in the pursuit of space, but I have included all the ipython notebook files (and ball rolling data) so you can see and try this yourself. You may need to install a few libraries including numpy, matplotlib, and the python control library (https://python-control.readthedocs.io/en/0.8.3/).

Next up, feedback.

• 2

The videos on youtube discussing state space control make it seems simple: choose where you want your poles and as long as your system is controllable, you can find a feedback matrix that will get you where you want to go. In practice, there are plenty of details that can trip you up.

My path through state space was pieced together with studying and trial and error. Lather, rinse, repeat. If there are any real control engineers, I would welcome your feedback.

Using the place() command. The poles were placed on the left hand plane for stability but not too negative given the relatively slow update frequency.

```poles = [-2.2, -2.]
Kfeedback = control.place(syst.A, syst.B, poles)

## which returns the following:
[[0.01257143   0.012 ]]```

The feedback matrix is 1x2 which when multiplied by the 2x1 full state variable, x, results in a scalar control input.

The new A matrix then becomes A - B * K

```feedbackA = syst.A - syst.B@Kfeedback ## feedback system A matrix
sysFeedback = control.ss(feedbackA, syst.B, syst.C, 0)

T ,yout = control.step_response( sysFeedback)
```

The step response of the closed loop system no longer accelerates off the table.

For the three step responses above, the poles were placed at (-5 and -6), (-2.2 and -2), and (-1 and -0.8) for the orange, blue and green curves respectively. The faster responses require more control inputs and may not be stable with a low refresh rate. So the medium speed poles were chosen for now.

In addition to using the control library to simulate the response, I wanted to confirm that I could simulate and later control the system. This turned out to be simpler than I expected. The code basically looks like this:

```x= np.matrix([[500], [0]])  ## initial value. 512 is max value of touchscreen.

yout = []
controls = []

for i in range(50):       ## simulate 5 seconds
u =  -Kfeedback @ x
x = sysd.A* x + sysd.B @ u  ## update system

yout.append(x[0,0])        ## append position
controls.append(u[0,0])    ## append control, u```

Which basically boils down to two steps:

• Calculate state feedback: u = -K * x
• And update system: x = A * x + B * u

In this simulation, the full state is available. In the ball platform, the position is measured from the resistive touchscreen and the velocity can be computed as the Δ x divided by Tstep. The proper way, however, is to use something called an observer.

Full disclosure: The actual order of events was that I tried to implement a Kalman observer/filter. It was trickier than I thought. So I fell back to a "Luenberger" observer. I still plan on going back to a real Kalman observer to correct for the glitches in the touchscreen input.

It turns out that the observer is itself a state space system! Who knew!? In this system, the state variable is xhat or the estimated state.

`xhat[k] = A * xhat[k-1] + B * U`

N.B. the A and B for the observer are derived from the system matrices but not the same! Same for the observer U which is now 2x1 in size, the command input and the measured position.

The observer is found by calculating an observer gain for given set of desired poles:

```poles = [-7, -8]
Kobs = control.place(syst.A.T, syst.C.T, poles).T```

The .T notation performs a matrix transpose. The poles were chosen to be faster than the desired control response-- we want to be able to observe faster than we control. The returned Kobs gain is 2x1. It will be used to calculate the observer state matrices:

Aobs = A - K * C

Bobs is concatenation of B and K, i.e. the 2x1 matrices side by side.

The estimated state, xhat, will be:

xhat[k] = A * xhat[k-1] + B * u

N.B. These observer A and B matrices are different from the system matrices. Furthermore, the input, u, for the observer system has been augmented with a measured x, i.e. it is now a 2x1 matrix containing the input u from the ball/platform system, as well as the measured ball position.

This now allows us to do full state feedback using an incomplete state measurement-- in this example, only a position. The steps are:

• Measure ball position
• Construct observer u, namely system u from last iteration, and measured ball position
• Feed this into the observer system to get new xhat which gives both position and velocity
• Use xhat to compute full state feedback u
• Control "real" system with this u

The plot above shows a manual simulation using an observer in the feedback loop. The green line is the command or desired position, the dashed red line is the xhat approximation, and the blue is the position of the "real system."

So the above code was converted into something the Arduino could handle and after a bit of fiddling, it would balance the ball. But the results were not what I expected, especially in regards to steady state error. The ball would stay on the table but thats about it. Deviation from ideal behavior, e.g. rolling friction, small offsets in table tilt, and imperfect model parameters made for lack-luster steady state response.

I had naively thought that state space control would automatically fix all such problems. It was a sobering realization. But what state space taketh away, state space giveth...

You can see the platform in action here:

BTW, all the python/jupyter notebooks used have been uploaded as well as the Arduino code.

Next up, robustness methods, namely, the addition of an integrator.

• 3
3. Robustness

So vanilla state feedback has problems with steady state error. It seems that one way of improving this is to use an integrator.

In broad brushstrokes, the plan is to compute an error signal and pass this through an integrator. This becomes an additional state, an accumulated error measurement. So now the state variable, x, goes from 2x1 to 3x1. There is a corresponding increase in size of the other state space matrices. You then choose feedback for this augmented state to make the integral of the error signal fall to zero. Boom. Easy.

In practice, how to do this was not no clear. It took a fair amount of searching and head scratching.

I found a page at the University of Michigan website that nudged me in the right direction. http://ctms.engin.umich.edu/CTMS/index.php?example=MotorPosition§ion=ControlStateSpace

This part was not the easiest to understand, so please forgive any mistakes. I am sharing what I learned in the hopes that it may help others. If there are any control engineers reading this, I would welcome your feedback. In particular, when you calculate feedback, should the calculation be done using done continuous time matrices even though it is going to be used in discrete time?! No correction to improve on the discrepancy?

We will call this new integrated error signal sigma. Because x is now 3x1, all of the system matrices will need to be "augmented."

```Aa = np.matrix( [[0, 1., 0.],
[0., 0., 0.],
[1., 0., 0.]] )
Ba =  np.matrix( [[0.],
[350.],
[0.]] )
Br = np.matrix( [[0.],
[0.],
[-1.]] )

Ca =  np.matrix( [[1., 0., 0.]] )```

## N. B.

It took a while to understand how to implement this because if you try to construct a system with Aa, Ba, and Ca, it will return a system of order 2. At least that's what the python control library does. Likely because the system as defined is uncontrollable, i.e. third column of Aa and third row of Ba are all zero making the new sigma variable uncontrollable.

The solution? There are two augmented B matrices! Two of them! Ba is used to calculate the gain as well as compute the closed loop augmented A matrix. But in the closed loop system, the Br matrix is used. Who thinks up this stuff?!!

Once the integrator is incorporated into the control loop, the step response looks very similar to the response without the integrator:

So what did we gain here? It looks indistinguishable from the original closed loop response? The difference comes in the disturbance response.

You can see the improves steady state response here (at 54 seconds):

I have uploaded the python notebook for anyone wanting to try this. The Arduino code is also available.

Up next, Kalman observer.

Share

## Similar Projects

Project Owner Contributor

### Controlling VLC with Arduino

8teims

Project Owner Contributor

### Omega_garage

James Harding

Project Owner Contributor

### Remote Chef

Tim van Iersel

Project Owner Contributor

Eric Cantrell

# Does this project spark your interest?

Become a member to follow this project and never miss any updates