FloatShield: MPC Control

Question

FloatShield: MPC Control

PeterChmurciak opened this issue 5 years ago · 12 comments

@gergelytakacs @mgulan @erik1392 I am experiencing some issues with matlab-arduino communication. I have not opened the matlab examples for a long time so I did not notice any change, but when I tried creating the LQ example there were some issues - matlab-arduino communication sometimes takes too long - which trips samplingViolation flag and ends the experiment. I have tested whole scripts with tic/toc and the culprits are functions FloatShield.sensorReadAltitude(); and FloatShield.actuatorWrite(u); - the only two functions that actually communicate with arduino. Ordinarily it takes ~0.01 second for each of them to be executed, so the whole script fits into the Ts = 0.025 second window, but other times it takes ~0.02 second each and in those cases even Ts = 0.040 is not enough. I am clueless about the cause of this, I have tried modifying baud rates in the matlab library, closing everything except matlab, looking through the forums, disabling antivirus and also installing updates for matlab and java but to no avail. When it occurs, not even matlab PID example can be run - it is not caused by script content but by the two communicating functions. It lasts for hours - in the morning it does not work, in the evening the same script works. Might be caused by matlab version, or version of matlab arduino support package, or maybe by some process running in the backgroud... As far as I am concerned it looks very random, currently I even suspect ambient temperature to be the culprit...

Do you have any clue what could be the cause ? Did you ever experience something simmiliar ?

Now for the main topic of this thread.

I have added matlab functions required to get linear MPC matrices for experiments and also to set constraints. They are "heavily" inspired by the ones from your book for TAR3, squeezed into only two functions and described for AutomationShield purposes.

With them (when I was currently not experiencing previously mentioned problems with samplingViolation) I was able to create matlab example that implements them with the FloatShield plant.

Penalisation matrices used were same as for LQ while the prediction horizon was set to 2. Had to restart the example few times as there were sometimes problems with sampling being violated, this time possibly even because of script content - use of quadprog and kalman estimate.

Results:

The "noisy" input is probably caused by the third state (air velocity) also mentioned in #210 (comment). But in my eyes it is as well the reason the ball is stable even in higher portions of the tube - in all other combinations (so far) of penalisation matrices, the ball became unstable in higher parts and the regulator was not able to stabilise it back.

The penalisation can be surely tweaked further, but for now I would like to try to create the same experiment in arduino. Could you give me some clues on how the quadprog, or something simmiliar that you possibly have experience with, can be implemented in arduino ?

Answer 1 · 2020-04-27T06:24:44.000Z

I have looked at Arduino_Unconstrained_MPC_Library and its second version Arduino_Constrained_MPC_Library, as they look pretty promising, but so far was not able to run even the template code.

On the page the autor says that it is also for Arduino hardware, but he was using Teensy 4.0 Platform to run it - the code might not be compatible with Arduino board.

So far I was not able to find any other libraries. Maybe I will have to try to make them work.

Answer 2 · 2020-04-27T16:22:37.000Z

@PeterChmurciak Good job so far :-) About quaprog, I am not sure if MATLAB's Optimization Toolbox supports code generation in a suitable form for Arduino. Even if it did, it will most likely not be efficient enough for real-time use. I would opt for something else, there should by several open-source solvers available by now. For example @gergelytakacs has some experience with μAO-MPC, see
http://ifatwww.et.uni-magdeburg.de/syst/muAO-MPC/
At a glance, it appears to implement a first-order algorithm, i.e. it may be relatively sub-optimal, but this does not matter in our case at all. The implementation seems quite easy, so you may have a look that. If you are not happy with it, let me know, I can recommend something else. Anyway, the "Arduino_Constrained_MPC_Library" does not seem to be super-efficient and robust either, more like a DIY stuff.

Answer 3 · 2020-05-05T22:23:30.000Z

@gergelytakacs @mgulan I was able to implement the MPC control with muAO-MPC that @mgulan mentioned in previous post. Using it I have created FloatShield_MPC example for Arduino IDE.

Results look:

(60 seconds spent on each level)

Results look pretty good.

Answer 4 · 2020-05-06T04:29:53.000Z

@PeterChmurciak they sure do🙂
Still, I think everyone will be wondering about that input profile. Can you provide a more detailed graph showing showing input (and output) around some of the set-point transitions?
Also can you log and plot the respective execution time?

Answer 5 · 2020-05-06T06:46:21.000Z

Super awesome job. @mgulan we discussed this live. I am not worried about the "near constant" input profile, see other thread and see our own paper!:)

@PeterChmurciak did an awesome job. I want his soul for science.

Answer 6 · 2020-05-06T06:57:58.000Z

@gergelytakacs I already recalled this during yesterdays paper reading; just wanted to see the details here👍🏻

@PeterChmurciak Well, are you on board?😄

Answer 7 · 2020-05-11T12:07:27.000Z

All right. So I have read this again. I have been a crappy Git user in the past couple weeks, but we've been discussing stuff with @PeterChmurciak during our live online meets.

Some thoughts that have been either said here or live

@PeterChmurciak uses a horizon of 2. It would be good to achieve at least 5, preferably 10. It would be also nice to see what are the hardware limits with different versions.
If quadprog will be eventually supported by Simulink for compilation, it will be huge and inefficient. So I would not really wait for it or build a future strategy for your thesis on it. It is fine for the MATLAB implementation though.
I fully understand with the muAoMPC suggestion from @mgulan . As far as your thesis goes this is pretty cool already. We can think of more things to do (as always) but lets write down what you have.
Concentrate on writing. Presenting your research formally may raise other questions related to your work in us. But all in all, very nice job.
I will try to dig deeper in your code in the upcoming days, so you might find some questions notes here.

Answer 8 · 2020-05-11T12:30:44.000Z

@PeterChmurciak Please also add the Python source for generating the MPC code to the example, I think that is relevant - not only the auto-generated code.

Answer 9 · 2020-05-12T07:50:00.000Z

@PeterChmurciak (FYI @mgulan)

I've been going through your latest work, mainly on LQ and MPC. I have went through your code too, obviously I can't test it as I don't even have an Arduino at home. I tested the ones in MATLAB not needing HW. Here are various fairly doable and minor(ish) chores and notes I'd like you to do and think about. I know, fekk me, I should've done this earlier - but here we go. :) Don't freak out, there is nothing major below. Remember, writing your thesis is still absolutely your main priority now.

So here we go:

You've mentioned you have used a horizon 2 MPC in your examples. Technically that is MPC, but I'd like you to try at least horizon 5 or even 10 and see if it works. If none of these works, you can possibly try to find out the maximum that is doable by the individual implementations.
Please show the Python source (muAOMPC) for the FloatShield example. I think you can just put there in the Arduino "examples" folder for now.
Just a note: Q, P is called process and output noise (not error) matrix
So, If I understand this correctly

 if (y >= Xr(0) * 2 / 3) {                                               
      break;                                                               
    }

Serves only to get the ball close to the first reference, before actually starting the control loop? Is that correct?

Think about the role of FloatShield_LQ_Gain.m . The upcoming bullet points will be about this. I think this file can 1. generate the gain for Arduino IDE use 2. Show what this gain does in a pure simulation.
FloatShield_LQ_Gain.m reproduces the Grey-Box identification example. I'm not sure duplicating the same thing in two files is wise. What I would do is to save a suitable output but the grey-box identification script as a .mat file and read that in, then go from there. Is there any "fine" tuning in your own identification? If yes, no problem, but it is still not necessary to repeat the entire thing... I think learners may find this confusing. Also there is a possibility to change the identification routine in one file, but not in the other. If you'd read in some pre-saved model, you could just start from line ~120 of the current code.
The code then goes on to tune the Kalman filter, and do a bit of validation work. I would actually separate this into, let's say, FloatShield_Kalman.m or FloatShield_Kalman_Validation.m something like this. The FloatShield_LQ_Gain.m file should only contain calculating the LQ gain based on system matrices/tuning, but it can also contain a simulational verification example... So, e.g. you can tune the LQ gain then do a simulation with it.
Wouldn't be wise to put function printSSMatrix(SSmatrix, name) into a function of its own into the base "AutomationShield" library? Maybe a function that outputs (any) matrix to a header file suitable for BLA...?
It seems that this has a universal value to it, and we can possibly use it elsewhere. Also, have a look at prbsGenerate() which, instead of printing it to screen by disp prints it to a header file. This could also be a way to be a bit more universal.
Now what I see in FloatShield_LQ is okay. I don't really see the same sort of duplicity. It has the right balance of "let's repeat things to teach" but "do not repeat too much to confuse". That one is nice.
estimateKalmanState is neat. Why do you write for SISO systems? It doesn't work for MISO/SIMO/MIMO? Otherwise, good job. You will also have to write all this down for your thesis so you'll learn a lot:)
I find your naming scheme for the MPC functionality a bit confusing. (Not great, not terrible). One is set, the other is get. The set, get naming is used to access and change private variables in OOP - some users may be bothered by that. But at least, I think they should be the same nameing ideas. Just a suggestion. Otherwise very nicely done! Good job.
Another tip. When you are trying MPC using purely MATLAB, and it does not seems to finish the computation within the allocated sample time, you may fine-tune quadprog and try if a different QP solver does the job better, or worst-case scenario even reduce tolerances to finish faster. If it works like this, fine, this is just a suggestion if you run into trouble.
Returning (yet again) to FloatShield_LQ_Gain.m and as I said earlier, this script could do a closed loop Simulation. In that case, wouldn't be interesting to create a pure simulation for MPC for the user? You know, for ones who would like to try it without the hardware. So something like FloatShield_LQ_Simulation.m or FloatShield_LQ_Sim.m (I'm not even sure what is the naming scheme we used before). If you really want to spice up things, you can do the simulation but with using the nonlinear model, which will be controlled by the linar MPC!:)
The Simulink part could/should need a little attention. As of the blocks - maybe you could include ready-made models there! So, e.g. a student can take that model of the FloatShield and start experimenting with it without the hardware. It could contain a simple linear SS but also a nonlinear identified one! Also you could include a purely simulation example for LQ, and possibly even MPC (you can call quadprog for simulation!)
Attempting to do MPC in Simulink and with the intent of actually generating the code to hardware is a though job, but not impossible. I think I would leave this as a last "cherry on top of the cake" option, after you have written everything. For this to do, you can either go to non-Uno hardware and use qpOASES or stay w/ the Uno and try to port muAO MPC there. Again, this is only after your writing is done.
Maybe there is a better word to note "light" implementation of blocks in Simulink? Dunno. Not a priority.

UPDATE 12 PM, 5/12

Has LQ/MPC been tested on other hardware than Uno/Mega? I have added the Arduino implementations to Travis CI, they compile for Zero and Due.
Can you please pinpoint any MATLAB functions that do not contain exotic functions like dlqr? I'd like to add them to the Travis bunch, but I can only launch them in Octave.

Answer 10 · 2020-05-13T14:01:45.000Z

@gergelytakacs @mgulan I am currently trying to increase the prediction horizon in the Arduino IDE FloatShield_MPC example, however there are issues with the memory and it does not look good.

On Arduino IDE, when the prediction horizon is set to 2, the global variables (with included compiled C code from python) take up 83% (1714/2048 bytes) of the dynamic memory on the device and there is already a warning about low memory and potential stability problems. But it somehow works.

However already when the prediction horizon is increased to 3 the global varibles take up 94% of the dynamic memory, and at prediction horizon 4 the global memory taken up is 107% - the sketch does not compile anymore.

I have tried the prediction horizon 3 with 94% dynamic memory full, as 3>2, the sketch does compile and can be uploaded but after a few seconds something breaks and everything stops...

It seems like the prediction horizon 2 on UNO with 83% dynamic memory taken up is unfortunately the only (working) choice.

I have tried to try the Arduino MEGA, as that is the only other board that can be used with FloatShield R1 for reasons mentioned in #190 (comment).

The prediction horizon 15 is the most the MEGA can run, with 95% full dynamic memory (7810/8192 bytes). I have then also tried prediction horizon 10 with 57% full dynamic memory, and the results looked better to me than in the prediction horizon 15 (95% memory taken), but I may have been biased, as I have seen the effects of full memory on UNO.

TL;DR
When it comes to FloatShield_MPC example in Arduino IDE:
The biggest prediction horizon the Arduino UNO can handle by using the μAO-MPC compiled C code, is 2.
The biggest prediction horizon the Arduino MEGA can handle is 15 (maybe rather smaller than 15).

I am not sure whether some memory could be saved somewere, but it probably would not be enough.

Answer 11 · 2020-05-15T08:14:05.000Z

@PeterChmurciak @mgulan

However already when the prediction horizon is increased to 3 the global varibles take up 94% of the dynamic memory, and at prediction horizon 4 the global memory taken up is 107% - the sketch does not compile anymore.

Makes sense as this is a 3 state model. You've reached the memory limit of the Uno by N=3.

I have tried the prediction horizon 3 with 94% dynamic memory full, as 3>2, the sketch does compile and can be uploaded but after a few seconds something breaks and everything stops...

...and you've reached the execution timing limit of the AVR at 16 MHz with horizon 2!:) To me this is - unfortunate - but very interesting and useful information. However, when you wrote

The prediction horizon 15 is the most the MEGA can run, with 95% full dynamic memory (7810/8192 bytes)

I found this very interesting. Now why do you think that it runs with Horizon 3 on mega but not on uno... That's weird. I'd very much like to find out the reason for this. Because my initial guess (as stated above) would be that you've reached the limit of execution time. And that is the same on both HW.

I have then also tried prediction horizon 10 with 57% full dynamic memory, and the results looked better to me than in the prediction horizon 15 (95% memory taken), but I may have been biased, as I have seen the effects of full memory on UNO.

Horizon 10 is fine. Plese test this well, and use this in your thesis.

Now, what you can try:

Mess with the iteration limit of muAOMPC to see if you can launch it with horizon 3.
Make sure that N=10 really, really works on the ATmega2560.
Try to extract somehow the execution times, but this is tricky since it adds more time for Serial. (It'd be great if you had a 'scope).
Try to disable Serial alltogether and see if it works with horizon 3.
Move to the Due and the Zero whithout the hardware. Meaning that you can still provide us (and the thesis) with useful information on what sort of memory will be required by the MPC application on Due and Zero, and whether it seems to run. So basically you do a "dry" test of MPC without the hardware to learn a bit about the memory needs and execution times.

Answer 12 · 2022-05-19T13:51:08.000Z

Float MPC has been created!