Go Down

### Topic: trigonometric function use (Read 246 times)previous topic - next topic

#### tomasstio_as

##### Feb 11, 2019, 10:24 pm
Hello all,

I am new to this forum and not sure if it is the right place to put my question. I am a student and I need to build an experiment for analyzing data of a sensor that will be a part of a control system later on.
I have an accelerometer & gyroscope sensor available to measure a force applied on dynamical system (to get the force for a known mass from the acceleration). the force is applied somewhere between the x-y-z coordinates of the sensor which span the force vector. intuitively I want to use cos and sin of the measurements on the x-y directions of the accelerometer and project it using the angle measured by the gyro. the question is if I can use the math function in the C code of the Arduino Uno program and what is the price (in terms of precision and processing time) of using these functions. and if we can use trigonometric functions in on a controller code why do we use linearization for systems like inverted pandulum etc. at all (I know there is a good reason).

#### jremington

#1
##### Feb 12, 2019, 02:14 amLast Edit: Feb 12, 2019, 02:16 am by jremington
Quote
the force is applied somewhere between the x-y-z coordinates of the sensor which span the force vector.
I have no idea what this means. Post a diagram.

Quote
project it using the angle measured by the gyro
Project what, on what? The gyro measures the rate of rotation, not the angle.

Quote
use the math function in the C code of the Arduino Uno program and what is the price (in terms of precision and processing time)
Yes, you can use trigonometric functions on the Arduino. They are slow on a 16 MHz processor and precision is limited to 32 bit floats.

Quote
why do we use linearization for systems like inverted pandulum

#### Idahowalker

#2
##### Feb 12, 2019, 03:29 amLast Edit: Feb 12, 2019, 03:39 am by Idahowalker
Hello all,

using the angle measured by the gyro.
You are going to find that the gyro is really a rate gyro. The gyro is going to give you the rate of precession. When the gyro stops its precession the gyro will read 0. The accelerometers will give you tilt angle. Gyros drift and accelerometers are noisy.  Integrating the readings from the gyros and accelerometers filters the data. If you are going to use an Arduino Uno, the best you can do is to use a complementary filter, which will work fairly well.

If you need better output results then your choices are a Kalman filter or a Quaternion/Euler Angle filter, these will not do very well on a Uno.

I have had great results with the MahonyQuaternionUpdate filter. There is, also the MadgwickUpdateFilter. You can get the code for the Quaternion Filters here: https://os.mbed.com/users/onehorse/code/MPU9250AHRS/file/4e59a37182df/MPU9250.h/

You will need to define these values for yourself based on your research into the MMU that you use:
Code: [Select]
`float deltat = 0.0f;                             // integration interval for both filter schemes#define Kp 7.50f// #define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral#define Ki 1.7f`

The values, of Kp and Ki were found through lots of digging into the data of the MPU9250 and then fine tuning those numbers based upon 100 hour runs per number tweak.

It is better to use a value for Kp and Ki as they adjust, via eInt over time to compensate for various drift factors with the Mahony filter:
Code: [Select]
`float eInt[3] = {0.0f, 0.0f, 0.0f};              // vector to hold integral error for Mahony method`

Conversion of the Quaternions into Euler Angles is done by the following formulas:

Code: [Select]
` MahonyQuaternionUpdate ( ax[0], ay[0], az[0], gx[0], gy[0], gz[0], mx[0], my[0], mz[0] );            float yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);            float pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));            float roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);`

The value of float deltat = 0.0f; needs to be done by your own routines. I do the following:
Code: [Select]
`unsigned long TimePast = micros();  unsigned long TimeNow = micros(); TimeNow = micros();            deltat = ((TimeNow - TimePast) / 1000000.0f);`

Read the gyros, accelerometers, do the Mahony, do the Euler angle conversion, do any other number crunching, and the last thing to do is
Code: [Select]
`TimePast = micros();`. The closer to the beginning and end of your code processing you can record time now and time past the more accurate your results will be; I hope that makes sense. deltat must be preserved between each iteration of your calculation. There is a easy way to use nano seconds, with a ESP32 but you'll get overflows in about 28 seconds that you have to compensate for. Which adds more code which increases the time between reads and calculations. I found it was not worth the effort.

A Arduino Due and a ESP32 has enough arse, from what I have used, to handle the calculations. The ESP32 takes about 290uS to do the Mahony, Euler, and other processing. I get great results with an ESP32, and reading the MPU9250 FIFO buffer, and processing the MPU info at 10 times a second.  I get a bit less than 30 FIFO samples per read, I average the contents of the FIFO buffer before sending the data to the Mahony.

I use the MPU data to create servo torque values to maintain a X/Y platform to level.

After all that, you can get fair results using a complementary filter and a Uno with data reads of about 1X or 2X a second. A search the net for a mmu complementary filter will get you the code.

A complementary (with low pass filter) can result in oscillating values, you can adjust those oscillations out, the K and K1 values, but you reduce sensitivity by doing so.

I had a few runs with the MahonyQuaternionUpdate where data oscillations occurred when I used a 0 for Ki.

#### tomasstio_as

#3
##### Feb 12, 2019, 02:56 pm
I must say I didn't expect such an helpful and detailed answer. Thank you so much your answer helps me a lot I really appreciate that. I can't thank you enough!!!

Go Up