[idea] Introduce a velocity vector

[Sebbi]

Hey,

currently MultiWii uses sensor data to calculate its orientation resulting in roll and pitch angles. If a magnetometer is present you also get a yaw angle (= heading). Additional sensor data can be height/altitude (from sonar, barometer and/or GPS, but only barometer data seems to be used in current versions) and velocity (also from the barometer, sonar, optical flow sensors, GPS and ever from the accelerometer).

However, the only time speed/velocity is calculated and used is in the GPS code, the waypoint code specifically.

I think some modes could be greatly improved if the multicopter would know it's speed and could adjust motors to change it accordingly.

Examples:
* level mode: this is basically a position hold using only the accelerometer. Position hold means the horizontal speed should be 0. If implemented this way level mode could even counteract wind to a certain amount.
* altitude hold mode: this is also a kind of position hold, but the vertical speed should be 0. The current implementation adjusts throttle directly without knowing anything about the speed of the copter, this makes it very dependend of the PIDs used.
* position hold mode: well, this is a position hold mode and needs GPS to hold an absolute position (there is no other sensor that could provide that short of some triangulation from groundstations or advanced computer vision). But GPS is not very accurate with small movements ... this could also be improved.

If we could merge the different sensor data in a more or less accurate global velocity vector the copter would always know how fast it is moving in a certain direction. This could be used as a base for the mentioned modes and even some new ones, e.g. some mode where the throttle stick controls the speed of ascent/descent instead of the throttle.

Problems i see:
* accelerometer data is very noisy because of vibrations. Only integrating them to get velocity will probably not work and introduce large errors if no other sensors are available to compensate. However, they should be ok for small time frames.
* i only know about kalman filters being used to merge sensor data like this, they aren't fast and hard to understand. There could be a simpler, less accurate way ...
* it will probably only be useful to pilots with additional sensors and GPS

What do you think?

First step: calculate vertical velocity and use it to improve the altitude hold mode

[Sebbi]

Ok, after some research I found the MultiWii version from alexmos (http://code.google.com/p/multiwii-alexm ... ii/IMU.pde) which has very promissing changes in it. It seems to calculate a vertical velocity from the accelerometer (and uses sonar).

I wrote a quick & dirty simulator to test different algorithms to get the vertical speed in a noisy environment and to hold the height as good as possible. Looks promissing, but needs some work and testing on the copter itself

[signal15]

This is what you're talking about:

https://en.wikipedia.org/wiki/Inertial_ ... ion_system

The navy has used these for a long time because when traveling underwater (or under Arctic Ice) there is no GPS signal, and there are no stars to look at.

[signal15]

So, would the Kalman filter be applied before sensor inputs were used at all? Or just before feeding them to the inertial nav code? If you're implementing it, I would think you might as well apply it so the resulting output is used everywhere. This would make vibration in general less of an issue than it is now.

[signal15]

Oooh, there is a bunch of stuff on doing Kalman filters on the arduino:

https://www.google.com/search?aq=0&oq=a ... man+filter

[Sebbi]

Exactly ... but MEMS sensors aren't drift free enough to make this work, as navigation with them would need the accelerometer data to be integrated twice and that introduces a lot of error. But it might be just accurate enough to get the velocity and use a Kalman or complimentary filter to correct the velocity from GPS, Sonar, optical flow, etc.

I am currently testing a MARG algorithm to calculate more accurate (and filtered) angles for level mode. I am not sure if this is the right way to go, but it definetly has no problems when the copter has odd angles like anything over 45 degrees pitch/roll (but it wont stay level yet, in level mode)
https://github.com/sebastianherp/MultiW ... MENTAL.ino

[signal15]

How much and how big are laser interferometer gyros?

[Sebbi]

The gyros aren't the problem (they seem pretty stable), but the accelerometer. Even the slightest bias offset leads to huge differences when integrating that data twice (to get the position) ... and with all the vibration and filtering done it doesn't get more reliable while flying.

[copterrichie]

The gyros aren't the problem (they seem pretty stable), but the accelerometer. Even the slightest bias offset leads to huge differences when integrating that data twice (to get the position) ... and with all the vibration and filtering done it doesn't get more reliable while flying.

I personally do not have the skills to write the software but my thought on this subject is, to use the accelerometer as Reference only and not part of the calculations. Kinda like a co-pilot in charge of keeping the aircraft orientated. So the ACC and Compass does not have to be sampled as often allowing for a greater smoothing logarithm.

[signal15]

If you look at the IMU code above, there are two scenarios coded into it. One with gyros only, and one with gyros and level mode. Taking into account level mode increased the time of one cycle to something like 5 times that of gyros only. Due to resource contraints, it might not be possible to do all of this with the accelerometers anyway. At least on the Atmega 168 or 368 or whatever is in most boards. The question is, would a Naze 32 board with it's better processor allow for more complex calcs?

[copterrichie]

I think the move to a 32 bit architecture is a positive move but I believe making the ACC a reference is workable on the 8 bit. Before the IMUs, a gyro only tricopter did an outstanding job at maintaining level as long as there was no major disturbance, it would drift and this is where the ACC would enter into the picture.

[Sebbi]

If you look at the IMU code above, there are two scenarios coded into it. One with gyros only, and one with gyros and level mode. Taking into account level mode increased the time of one cycle to something like 5 times that of gyros only. Due to resource contraints, it might not be possible to do all of this with the accelerometers anyway. At least on the Atmega 168 or 368 or whatever is in most boards. The question is, would a Naze 32 board with it's better processor allow for more complex calcs?

What IMU code above? Level mode doesn't increase cycle time significantly, only compiling MultiWii with ACC defined does.

I think the move to a 32 bit architecture is a positive move but I believe making the ACC a reference is workable on the 8 bit. Before the IMUs, a gyro only tricopter did an outstanding job at maintaining level as long as there was no major disturbance, it would drift and this is where the ACC would enter into the picture.

It definetly is workable on 8bit and it does work ... kind of My idea was to have a global velocity vector to enable new flight modes and improving existing ones. Velocity would be primarily calculated from the accelerometer, but should be mixed/corrected (the "copilot") with other measurements of height (Baro, GPS, Sonar) and movement (GPS, opticalflow). However, I am currently looking into the getEstimatedAttitude() code ... attitude and heading of the copter need to be as precise as possible first to then get a usable velocity vector from the accelerometer