System Identification

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.
Software download
Post Reply
User avatar
alduxvm
Posts: 40
Joined: Thu Apr 25, 2013 2:25 pm
Contact:

System Identification

Post by alduxvm »

Good day everyone!!

Let me tell you a bit about my project, before I start asking questions and suggestions :P

As the subject implies, I doing systems identification of multicopters being flown by me via a multiwii board. Why do I want to do this? I want to do very precise navigation algorithms using a motion capture system.

Systems identification is a statistical method to build mathematical models of the multicopters, to have excellent control we require a perfect mathematical model. In order to do a good sysid we require data, lots of data, data coming from the board as well as the pilot and the position in the space (motion capture), so, I require raw imu (accelerometers and gyroscopes), pilot commands (rc channels) and position (x,y,z).

So far, I have a position controller working with the multiwii board being flow by simulink using data from the motion capture system and just sending rc channels via a 3DR robotics radio (roll, pitch, yaw, throttle), you can see a video about that here:

[vimeo]https://vimeo.com/105761692[/vimeo]

I works nice... but I want it to work even nicer and better!!! so, we need all the mathematical models and parameters to be as precise as possible, ergo, systems id.

I knew that the 3dr radio was not good enough to send data in a fast way to the ground station... So, I put onboard a raspberry pie, this computer ask data to the multwii and also to the motion capture system, and saves it... thats it.

Multiwii and RPI
Multiwii and RPI


My code is written in python and everyone can have access to it: https://github.com/alduxvm/rpi-mw/

Its working nice... but its still not good enough, allow me to explain why.

The fastest I can go asking and then getting data is ~ 0.02 seconds, aprox 50hz, which is the PERFECT sample rate to do sysid... buttttttt I need MSP_RAW_IMU as well as MSP_RC, when doing that, I got a delay between each reading of ~ 0.06 which is 16hz... and NOT GOOD ENOUGH for sysid.

This is a sample of the data:

Data sample
Data sample


What I do in order to save data is as follows:

- Ask mw for raw-imu
- wait for the response
- save response
- Ask for rc channels
- wait for response
- save response
- repeat

The waiting response is fixed and its at the moment 0.02... if I do less I got garbage/corrupted data... So, first question, is there a way to improve this???

Of course a main issue is that I need to ask for raw and rc in a separate way, this makes more delay, so, I wanted to create a new message, one combining raw and rc... and looking at "Protocol.cpp" tells me the next:

//to multiwii developpers/committers : do not add new MSP messages without a proper argumentation/agreement on the forum

hehehe, of course, that do not stop me to test, so, I tried that, I modify this part:

Code: Select all

case MSP_RC:
     headSerialReply(RC_CHANS*2+18);
     s_struct_s((uint8_t*)&rcData,RC_CHANS*2);
     s_struct_s((uint8_t*)&imu,18);
     break;


and just a s_struct without headSerialReply... for the while :P

The problem with this is that I have a response with blank spaces... so, Im not sure if this is the good way to implement what I want, I´m getting something like:

Code: Select all

40 $M>"i????????????]??l?????
40 $M>"i??????????????u??x?p?
40 $M>"i????????w???|$?x?p?
40 $M>"i????????]#_?'?x?p?
40 $M>"i????????>(4??y???)???j
40 $M>"i????????4=*?U??)???o
40 $M>"i????????
                +?????)???^
40 $M>"i??????????????J?4???8??p
40 $M>"i????????????????0???8???
40 $M>"i??????????T???????8???
40 $M>"i????????P??Rg?????R?
40 $M>"i????????;d????m????R0
40 $M>"i????????ul?????????R?


Of course, I havent decoded the message yet, so, its in its pure form :P... also, by any chance, has anyone use the module struct on python to decode this binary data?

Ok... So, added to this, I have very noisy accel data... almost useless for systid... Do the LPF on the MPU6050 that is configurable affects the accel as well? because it states that is for gyro.

Accel
Accel


So, I have 3 main issues:

- Improve the RAW_IMU and RC readings to be at least 50hz
- Make new msp message with RC and RAW to improve, without errors
- Filter accel readings (I´m testing next week a mechanical vibration dampening similar to the ones I use for APM)

Thanks a lot!!

So sorry for the long post.

Cheers!

User avatar
treym
Posts: 258
Joined: Sat Jul 21, 2012 12:28 am

Re: System Identification

Post by treym »

The fastest I can go asking and then getting data is ~ 0.02 seconds, aprox 50hz, which is the PERFECT sample rate to do sysid... buttttttt I need MSP_RAW_IMU as well as MSP_RC, when doing that, I got a delay between each reading of ~ 0.06 which is 16hz... and NOT GOOD ENOUGH for sysid.





the idea is to either have 2 thread , or to be in sync with the fc, the os , the python , the usb , etc .. this latter option is not that harder and seem to be more efficient,

some data : https://gist.github.com/treymarc/f1849049ede3ccfb4f6f ~ 100hz :)



Code: Select all

case MSP_RAW_IMU:

         data.ax = read16();
         data.ay = read16();
         data.az = read16();

         data.gx = read16() / 8;
         data.gy = read16() / 8;
         data.gz = read16() / 8;

         data.magx = read16() / 3;
         data.magy = read16() / 3;
         data.magz = read16() / 3;
         long now = System.currentTimeMillis();
         multiPlanner.triggerUpdate();
         Log.d("MSP",String.format("%d", (int)(now-last)));
         last = now;
         break;


Code: Select all

case MSP_ATTITUDE:
            askMsp(MSP.MSP_ATTITUDE);
            askMsp(MSP.MSP_RAW_IMU);
            break;

User avatar
alduxvm
Posts: 40
Joined: Thu Apr 25, 2013 2:25 pm
Contact:

Re: System Identification

Post by alduxvm »

Thanks Marc!!

Is there a modification on the MW code for this tests?? or everything is on the "reader" side??

also, in the second part, does your function "askMsp" clears (flush) the serial port?? and do you put a sleep or small delay on it???

User avatar
alduxvm
Posts: 40
Joined: Thu Apr 25, 2013 2:25 pm
Contact:

Re: System Identification

Post by alduxvm »

Ok, I manage to do the first part!!!

My objective was:

- Custom MSP for sending the first 4 RC channels and ACC and Gyro data
- Receive it and process it as fast as possible, at least 50hz

How I solve it:

- I had to make some changes on the MW code, just add a couple of functions to Protocol.cpp and change the TX_BUFFER_SIZE...

Code: Select all

//in Serial.h

#define TX_BUFFER_SIZE 256


Code: Select all

//in Protocol.cpp
.
.
.
#define MSP_RC_RAW_IMU           121   //out message         4 rc channels and 6 DOF (ACC and Gyro)
.
.
.
void  s_struct_s(uint8_t *cb,uint8_t siz) {
  while(siz--) serialize8(*cb++);
}
.
.
.
case MSP_RC_RAW_IMU:
     headSerialReply(8+12);
     s_struct_s((uint8_t*)&rcData,8);
     s_struct_s((uint8_t*)&imu,12);
     break;
.
.
.



And did major changes to my python code, to avoid the horrible time.sleep(0.02) that I had before... So, the maximum speed I can get for any MSP is 0.016, thats roughly 62.5 hz which is good to do systems ID... This is the speed that Marc mention before to me :), thanks for the pointers Marc!!

I'm not sure if the change to the buffer will affect the performance inflight of the MW but tomorrow I'll run a test and of course present all data on this forum for people interested...

I still need to check if the accelerometer data is good to make id.. so, tomorrow I'll fight with noise :P

I will also make a python module to ask and receive data from MW, for the people interested as well.

User avatar
alduxvm
Posts: 40
Joined: Thu Apr 25, 2013 2:25 pm
Contact:

Re: System Identification

Post by alduxvm »

ok, I made lots of modifications and finish up doing a new project :P

https://github.com/alduxvm/pyMultiWii

there you go guys...

on my mac I'm getting data at 62.5hz... it looks like this:

Code: Select all

{'gz': 0.0, 'gy': 0.0, 'gx': 1.0, 'ay': -20.0, 'ax': -22.0, 'az': 520.0}
0.016
{'gz': 0.0, 'gy': 0.0, 'gx': 0.0, 'ay': -22.0, 'ax': -17.0, 'az': 513.0}
0.016
{'gz': 0.0, 'gy': 0.0, 'gx': 1.0, 'ay': -20.0, 'ax': -20.0, 'az': 518.0}
0.016
{'gz': -1.0, 'gy': 1.0, 'gx': 0.0, 'ay': -20.0, 'ax': -18.0, 'az': 518.0}
0.016
{'gz': 0.0, 'gy': 0.0, 'gx': 0.0, 'ay': -28.0, 'ax': -20.0, 'az': 515.0}
0.016
{'gz': 0.0, 'gy': -1.0, 'gx': -1.0, 'ay': -25.0, 'ax': -17.0, 'az': 520.0}
0.016
{'gz': 1.0, 'gy': 0.0, 'gx': -2.0, 'ay': -23.0, 'ax': -19.0, 'az': 513.0}
0.016


the 0.016 is the time in each reading... so, 62.5hz Happy times!!

but on the rpi, the history is different... I have no idea why is so different... It gets hang waiting on the while of readings and do weird stuff... maybe my communication logic is the one is wrong... so, anyone think this logic for reading a command is wrong???

Code: Select all


def getData(cmd):
    global rcChannels,rawIMU,attitude,temp
    try:
        sendCMD(0,cmd,[])
        time.sleep(0.01)
        timeout = time.time() + 0.04
        while True:
            header = ser.read(3)
            if header == '$M>':
                break
            if time.time() > timeout:
                ser.flushInput()
                ser.flushOutput()
                sendCMD(0,cmd,[])

        datalength = struct.unpack('<b', ser.read())[0]
        code = struct.unpack('<b', ser.read())
        data = ser.read(datalength)
        temp = struct.unpack('<'+'h'*(datalength/2),data)
        evaluateCommand[code[0]]()
        ser.flushInput()
        ser.flushOutput()

    except Exception, error:
        pass



cheers!

User avatar
alduxvm
Posts: 40
Joined: Thu Apr 25, 2013 2:25 pm
Contact:

Re: System Identification

Post by alduxvm »

Alright guys, if someone is interested, here is a better-shorter version of my code, for you to try, and also a small video for you guys to see it in action:

https://www.youtube.com/watch?v=TpcQ-TOuOA0

My code:

https://github.com/alduxvm/pyMultiWii


Cheers!!

Post Reply