adding roll pitch yaw example, including processing.org sketch

[owennewo]

This example provides good quality pitch, roll, drift from raw gyro/accel data

• gyro calibration to remove gyro drift
• trigonometry calculations to convert accel m/s/s to angles
• and a complementary filter to combine gyro/accel for better results

There is also a 'processing development environment' sketch to show this data in a 3d model. This is useful to demonstrate why gyro calibration is necessary and why a complementary filter provides better results than gyro/accelerometer alone.

@sandeepmistry - feel free to include or change this sketch as part of the LSM6DS3 library

[per1234]

Please run a Tools > Auto Format on RollPitchYaw.ino. That will give it the standard code formatting used on all Arduino example sketches.

[owennewo]

@per1234 - I've pushed a new commit with your suggestion. I've also moved the processing sketch because arduino editor seems to want to open the sketch because arduino has historically used 'pde' extension.

[per1234]

Thanks @owennewo! I have one additional suggestion: the Arduino IDE has a quirk in that when you do a "Save As", it will only save certain files and folders to the new sketch location. With the current system of putting the Processing files under the processing subfolder of the example sketch folders, a "Save As" will not bring along the contents of the processing subfolder to the new sketch location. The Arduino IDE does preserve all the contents of the data subfolder of the sketch through "Save As". So I would suggest moving the Processing files to the data/processing subfolders of each of the example sketch folders.

[owennewo]

Thanks @owennewo! I have one additional suggestion: the Arduino IDE has a quirk in that when you do a "Save As", it will only save certain files and folders to the new sketch location. With the current system of putting the Processing files under the processing subfolder of the example sketch folders, a "Save As" will not bring along the contents of the processing subfolder to the new sketch location. The Arduino IDE does preserve all the contents of the data subfolder of the sketch through "Save As". So I would suggest moving the Processing files to the data/processing subfolders of each of the example sketch folders.

Done

Requested changes have been made. Thanks!

[MichaelFBA]

Thanks for the code, I have adapted your code for an ardunio nano 33 ble. Arduino_LSM9DS1
May I ask how you would add the additional mag sensor data? Just wondering if it would improve the yaw drift?

[owennewo]

@MichaelFBA I'm using the nano iot 33 which has no mag sensor. I chose it for the wifi - although mag would have been useful.

Answer to your question - yeh the mag sensor would help yaw drift. It's even possible to get an accurate 'true north' but this is dependent on your latitude/longitude. If it's just yaw drift you want to fix and/or magnetic north is 'good enough' then you just need to do some startup calibration followed by another complementary filter.

It would basically work as follows:

1. calibration. On setup read magnetic yaw 10 times, once every 10ms. Take an average. This is your starting yaw.
2. initial gyro offset. Read gyro yaw this is your gyro offset. From this point on you are only interested in the change in gyro yaw.
   Each sample you take would be as follow

loop every 10 ms
new yaw = (old yaw + change in gyro yaw since last sample) * 0.98 + (new magnetic yaw) * 0.02
old yaw = new yaw

The above complementary filter is basically giving the gyro a 98% weighting and the mag a 2% weighting. You can play with these ratios. The 0.02 will smooth out a noisy mag sample but will completely remove drift over time - e.g. if yaw value is wrong by 10% then over 50 samples - most of that drift will be removed.

I've skipped over how to turn the mag reading into a yaw angle. I suspect it works similar to how you turn accelerometer reading from directional m/s/s to angle. Some trigonometry might be required.
Or perhaps it already returns angle?

A much simpler alternative is to just use mag for yaw. If you don't mind a bit of noise or lag (you could average last 5 yaw samples to get rid of some noise) then it'll probably be good enough.

[cansik]

This is an integer division, but looking at the result, this should rather be a float division. If lastFrequency is 0, because lastInterval is too large, you get INFINTIY values in your later calculations.

[owennewo]

I've changed it to:

float lastFrequency = (float) 1000000.0 / lastInterval;

Given up on this PR being merged tbh (It's a year old) - and the build failure below is the cherry on the top. It's trying to compile it against an UNO by the looks of it. :(

[cansik]

Yeah I totally understand that. I was just stumbling over it and used your code and found the fix. Leaving it here for others may try to use it. Thanks for your work!

[EnricOlmosRoca]

Hello Owen. How are you? I,m very interested in the YAW in degrees units. Thank you for your code. Works. But I,m not sure if I doing something wrong, or if it's normal that the Yaw angle always goes up. Means, If I don,t move the nano 33 iot,
the value is constantly increasing. Please, can you help me?

[owennewo]

Tldr - Drift with yaw is expected. You need a 9dof sensor but this chip only has a 6dof sensor

Pitch and roll show no drift because we blend gyro data with accelerometer data. The accellerometer measures gravity which is in a known direction (down!) when pitch and roll are 'zero'. Any change to pitch and roll results in grabity no longer being 'down'. A simple bit of trigonometry can yield absolute pitch and roll. Unfortunately when you 'yaw' the change in direction can not be sensed by accelerometer (as gravity is still down). To get accurate yaw, you'll need to blend a magnetic field sensor to measure magnetic north of the earth. Magnetic sensors can get confused by other magnetic sources e.g motors so may not be perfect. There are some great 9degrees of freedom sensors that contain gyro + accelerometer + magnetic sensor. E.g imu-9150 (or 9250).
Why is gyro not enough on its own? Gyro gives you the change in yaw angle (not the absolute yaw angle). We approximate the absolute angle by adding up these changes in angle over time (integration) but there is inevitably drift.
If you want to get really fancy, robotic cameras are able to work out yaw by tracking points of interest (e.g a corner of a box) and how it moves between frames.

The nano 33 ble and nano ble sense have a 9dof but not sure if someone has written the code (trigonometry) to get accurate yaw. You may better off looking at an external imu-9150. That will require you to use i2c interface to communicate with sensor (you'll find lots of examples).

[ghost]

Hello Owen, awesome work ! This example is very useful.
By the way, in the official documentation it says that the IMU.readAcceleration() function returns the acceleration in g.
After reading your example, I was confused by this comment :
// units m/s/s i.e. accelZ if often 9.8 (gravity)
Is this an oversight from a previous version?

[owennewo]

You may well be right. It's been ages since i worked on this. Easy to find out though! If accelz is 9.8 I'm right, if value is 1.0 I'm wrong.

This PR / example is just rotting, they may as well just lock the repo if they never merge PRs. I thought Arduino were good at accepting PRs.

[RobBotic1]

Hi Owen - excellent work and I'm disappointed it has not been incorporated - I find it very useful!
Question for you - looking at the calibrateIMU() function in RollPitchYaw.ino, how is the timing of the drift accounted for? In other words, gyroDriftX, gyroDriftY, and gyroDriftZ all appear to be in units of dps / refresh rate. But when they are used later, they are simply subtracted from a current reading. Is the assumption that the reading will be made constantly (i.e., lastInterval always approximately equals refresh rate)?
I ask as I would like to generalize for readings with delays, but I'm thinking then the refresh rate would need to be accounted for in the calculation of drift. Is my understanding correct?

[owennewo]

Hi @RobBotic1. gyroX, gyroY and gyroZ have units of angular velocity i.e rad/s and hence time is not important.

During calibration when the board is still, you would expect these to be 0 rad/s as the board is not rotating however gyros are typically off by a small amount e.g it thinks it is rotating slowly. By taking 100s of samples during calibration we can find the average 'drift' of velocity in each axis. Later will subtract this drift when making real readings. E.g. if we read gyroX and get 7 rad/s and gyroX drift is 0.1 then 6.9 rad/s is likely to be a more accurate value.

A small amount of drift is fine for x and y as complementary filter will fuse accX and accY which will slowly correct drift. The poor z axis has no complementary filter, that would require magnetometer which this board lacks

[CLAassistant]

All committers have signed the CLA.

[LazaroFilm]

Any updates on this pull request? this would solve all the issues I'm having right now.

[fralbo]

Sorry, but why do you compute roll/pitch from acceleration and not gyro ??
Doing that may result in good result on table but if you use on motorbike or drone, that's really bad as the IMU is always submitted to z acceleration only, even in turn because of lean angle.