## getEstimatedAttitude: Idea

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.
ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

### getEstimatedAttitude: Idea

Hi,
I have small idea, how we could simplify/optimize/etc existing Complimentary filter in WMC. As I understand corrently we have the following procedure:

1) Calculate rotation grom Gyro in angles
2) Add this rotation in angles to the previous estimate.
3) Create normalized vector RGyro using angles (Here we need cos/tan)
4) Calculate normalized vector for ACC
5) Apply filter (Weights) and get Estimated vector
6) Get Attitude of the Estimated vector (Here we need atan2)

My idea is to mimize conversions angles <-> vector. Basically what we need is just "rotate" estimated vector using current Gyro values (change from the last read) using Rotation Matrix, or cosine matrix. To do that we need to calculate sin() and cos() of the GyroDeltaAngle.From other point of view we probably can assume that GyroDeltaAngle is relativelly small from the last read and we can use small angle approximation: sin(x)~x, cos(x)~1. This assumption sugnificantly simplify calculations. So algorithm would be:
1) Calculate rotation grom Gyro in angles
2) Rotate estimated vector
3) Apply filter (Weights)
4) Get Attitude of the Estimated vector

I have created small prototype:

Code: Select all

``typedef struct {  float X;  float Y;  float Z;} t_int_vector_def;typedef union {  float          A;  t_int_vector_def V;} t_int_vector;#define ACC_LPF_FACTOR 8#define GYR_CMPF_FACTOR 300.0// Small angle approximation#define ssin(val)  (val)#define scos(val) 1.0#define stan(val) (val)#define GYRO_SCALE (200.0f * PI)/((32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.5f// 1.5 is emperical, not sure what it meansvoid getEstimatedAttitude(){  uint8_t axis;    static t_int_vector EstV = {0,0,0};  static t_int_vector deltaGyroAngle;  static uint16_t PreviousTime;  uint16_t CurrentTime  = micros();  uint16_t deltaTime = CurrentTime - PreviousTime;  PreviousTime = CurrentTime;  // Initialization  for (axis = 0; axis < 3; axis++) {    // LPF for ACC values    accSmooth[axis]         = (accSmooth[axis] * (ACC_LPF_FACTOR - 1) + accADC[axis]) / ACC_LPF_FACTOR;    // Gyro angle change since last time.    deltaGyroAngle.A[axis]  = gyroADC[axis] * (deltaTime * GYRO_SCALE);  }  // Rotate Estimated vector, ROLL  EstV.V.Z =  scos(deltaGyroAngle.A[ROLL]) * EstV.V.Z - ssin(deltaGyroAngle.A[ROLL]) * EstV.V.X;  EstV.V.X =  ssin(deltaGyroAngle.A[ROLL]) * EstV.V.Z + scos(deltaGyroAngle.A[ROLL]) * EstV.V.X;  // Rotate Estimated vector, PITCH  EstV.V.Y =  scos(deltaGyroAngle.A[PITCH]) * EstV.V.Y + ssin(deltaGyroAngle.A[PITCH]) * EstV.V.Z;  EstV.V.Z = -ssin(deltaGyroAngle.A[PITCH]) * EstV.V.Y + scos(deltaGyroAngle.A[PITCH]) * EstV.V.Z;  // Apply complimentary filter  for (axis = 0; axis < 3; axis++)    EstV.A[axis] = (EstV.A[axis] * GYR_CMPF_FACTOR + accSmooth[axis]) / (GYR_CMPF_FACTOR + 1);  // Attitude of the estimated vector    angle[ROLL]  =  atan2(EstV.V.X, EstV.V.Z) * 570.29577951;  angle[PITCH] =  atan2(EstV.V.Y, EstV.V.Z) * 570.29577951;}``

Small notes on prototype:
1) It can be simplified
2) Is still uses floats, but if it works it would be possible to convert it to the integer math
3) It is not nececary to normalize vectors for filter as it would automatically scale EstV to the ACC values in firtst couple of iterations(due the fact, that rotation not changing magnitude of the vector). And we can economize on sqrt().

Regards,
ziss_dm

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

### Re: getEstimatedAttitude: Idea

Hi,
This is slightly optimized version with atan2 approximation. Currently it gives cycle time ~2800.

Code: Select all

``typedef struct {  float X;  float Y;  float Z;} t_int_vector_def;typedef union {  float          A;  t_int_vector_def V;} t_int_vector;#define ACC_LPF_FACTOR 16#define GYR_CMPF_FACTOR 300.0// Small angle approximation#define ssin(val)  (val)#define scos(val) 1.0#define stan(val) (val)#define squared(val) (val * val)#define GYRO_SCALE (200.0f * PI)/((32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.5f// 1.5 is emperical, not sure what it meansuint16_t _atan2( float y, float x ){  #define ONEQTR_PI  M_PI / 4.0  #define THRQTR_PI  3.0 * M_PI / 4.0  float r, angle;  float abs_y = fabs(y) + 1e-10f;      // kludge to prevent 0/0 condition  if ( x < 0.0f ) {    r = (x + abs_y) / (abs_y - x);    angle = THRQTR_PI;  } else {    r = (x - abs_y) / (x + abs_y);    angle = ONEQTR_PI;  }  angle += (0.1963f * r * r - 0.9817f) * r;  angle *= 572.9577951;  return (y < 0.0f) ? -angle : angle;}void getEstimatedAttitude(){  uint8_t axis;  float  AccMag = 0;  static t_int_vector EstV = {0,0,200};  static t_int_vector deltaGyroAngle;  static uint16_t PreviousTime;  uint16_t CurrentTime  = micros();  uint16_t deltaTime = CurrentTime - PreviousTime;  PreviousTime = CurrentTime;  // Initialization  for (axis = 0; axis < 3; axis++) {    // LPF for ACC values    accSmooth[axis] = (accSmooth[axis] * (ACC_LPF_FACTOR - 1) + accADC[axis]) / ACC_LPF_FACTOR;    AccMag += square(accSmooth[axis]);    // Gyro angle change since last time.    deltaGyroAngle.A[axis]  = gyroADC[axis] * (deltaTime * GYRO_SCALE);  }  // Rotate Estimated vector, ROLL  EstV.V.Z =  scos(deltaGyroAngle.A[ROLL]) * EstV.V.Z - ssin(deltaGyroAngle.A[ROLL]) * EstV.V.X;  EstV.V.X =  ssin(deltaGyroAngle.A[ROLL]) * EstV.V.Z + scos(deltaGyroAngle.A[ROLL]) * EstV.V.X;  // Rotate Estimated vector, PITCH  EstV.V.Y =  scos(deltaGyroAngle.A[PITCH]) * EstV.V.Y + ssin(deltaGyroAngle.A[PITCH]) * EstV.V.Z;  EstV.V.Z = -ssin(deltaGyroAngle.A[PITCH]) * EstV.V.Y + scos(deltaGyroAngle.A[PITCH]) * EstV.V.Z;  // Apply complimentary filter (Gyro drift correction)  // If accel magnitude >1.4G or <0.6G => we neutralize the effect of accelerometers in the angle estimation.  // To do that, we just skip filter, as EstV already rotated by Gyro  AccMag =  AccMag / square(acc_1G);  if (!(0.36f > AccMag or AccMag > 1.96f)) {    for (axis = 0; axis < 3; axis++)      EstV.A[axis] = (EstV.A[axis] * GYR_CMPF_FACTOR + accSmooth[axis]) / (GYR_CMPF_FACTOR + 1);  }  // Attitude of the estimated vector    angle[ROLL]  =  _atan2(EstV.V.X, EstV.V.Z);  angle[PITCH] =  _atan2(EstV.V.Y, EstV.V.Z);}``

Alexinparis
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

### Re: getEstimatedAttitude: Idea

Hi,

I've just tried it, and it works well. It's also a smaller code It's a very interesting observation !

It would be perfect with:
- the small angle approximation (one of the fastest code)
- the compass integration (the heading value for small angle only would be ok)

thanks, I think this is a part I will change soon ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

### Re: getEstimatedAttitude: Idea

Hi Alex,
It is quite simple to implement small inclination calculations, but is it really good idea? Basicaly it would increase control loop timing and decrise system response time for big angles. And PIDs would not be optimal any more. From my expirience: I can tune PIDs for level flight and it really stable even in wind gusts, but as soon I inclinate quad, it start oscilating. I'm not 100% sure that it is due changes in control loop timing, maybe something else wrong, but this is my current expirience. Maybe it is better to make control loop timing as stable as possible?
About magnetometer, could you please explain the basic idea how we want to use it? Do we want it to replace Accelerometer or use it only for Heading or implement combined filter for all 3 sensors?. Without too much thinking, it could be something like this:

Code: Select all

``typedef struct {  float X;  float Y;  float Z;} t_int_vector_def;typedef union {  float          A;  t_int_vector_def V;} t_int_vector;#define ACC_LPF_FACTOR  16#define MG_LPF_FACTOR   4#define GYR_CMPF_FACTOR 300.0// Small angle approximation#define ssin(val)  (val)#define scos(val) 1.0#define stan(val) (val)#define squared(val) (val * val)#define GYRO_SCALE (200.0f * PI)/((32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.5f// 1.5 is emperical, not sure what it meansuint16_t _atan2( float y, float x ){  #define ONEQTR_PI  M_PI / 4.0  #define THRQTR_PI  3.0 * M_PI / 4.0  float r, angle;  float abs_y = fabs(y) + 1e-10f;      // kludge to prevent 0/0 condition  if ( x < 0.0f ) {    r = (x + abs_y) / (abs_y - x);    angle = THRQTR_PI;  } else {    r = (x - abs_y) / (x + abs_y);    angle = ONEQTR_PI;  }  angle += (0.1963f * r * r - 0.9817f) * r;  angle *= 572.9577951;  return (y < 0.0f) ? -angle : angle;}void getEstimatedAttitude(){  uint8_t axis;  float  AccMag = 0;  static t_int_vector EstV = {0,0,200};  static t_int_vector EstM = {0,200,0};  static t_int_vector deltaGyroAngle;  static uint16_t PreviousTime;  static int16_t mgSmooth;  //projection of smoothed and normalized magnetic vector on x/y/z axis, as measured by magnetometer  uint16_t CurrentTime  = micros();  uint16_t deltaTime = CurrentTime - PreviousTime;  PreviousTime = CurrentTime;  // Initialization  for (axis = 0; axis < 3; axis++) {    // LPF for ACC values    accSmooth[axis] = (accSmooth[axis] * (ACC_LPF_FACTOR - 1) + accADC[axis]) / ACC_LPF_FACTOR;    AccMag += square(accSmooth[axis]);    // Gyro angle change since last time.    deltaGyroAngle.A[axis]  = gyroADC[axis] * (deltaTime * GYRO_SCALE);    #if defined(HMC5843)      mgSmooth[axis] = (mgSmooth[axis] * (MG_LPF_FACTOR - 1) + magADC[axis]) / MG_LPF_FACTOR;    #endif   }  // Rotate Estimated vector, ROLL  EstV.V.Z =  scos(deltaGyroAngle.A[ROLL]) * EstV.V.Z - ssin(deltaGyroAngle.A[ROLL]) * EstV.V.X;  EstV.V.X =  ssin(deltaGyroAngle.A[ROLL]) * EstV.V.Z + scos(deltaGyroAngle.A[ROLL]) * EstV.V.X;  // Rotate Estimated vector, PITCH  EstV.V.Y =  scos(deltaGyroAngle.A[PITCH]) * EstV.V.Y + ssin(deltaGyroAngle.A[PITCH]) * EstV.V.Z;  EstV.V.Z = -ssin(deltaGyroAngle.A[PITCH]) * EstV.V.Y + scos(deltaGyroAngle.A[PITCH]) * EstV.V.Z;  // Apply complimentary filter (Gyro drift correction)  // If accel magnitude >1.4G or <0.6G => we neutralize the effect of accelerometers in the angle estimation.  // To do that, we just skip filter, as EstV already rotated by Gyro  AccMag =  AccMag / square(acc_1G);  if (!(0.36f > AccMag or AccMag > 1.96f)) {    for (axis = 0; axis < 3; axis++)      EstV.A[axis] = (EstV.A[axis] * GYR_CMPF_FACTOR + accSmooth[axis]) / (GYR_CMPF_FACTOR + 1);  }  // Attitude of the estimated vector    angle[ROLL]  =  _atan2(EstV.V.X, EstV.V.Z);  angle[PITCH] =  _atan2(EstV.V.Y, EstV.V.Z);  #if defined(HMC5843)    // Rotate Estimated vector, YAW    EstM.V.X =  scos(deltaGyroAngle.A[YAW]) * EstM.V.X - ssin(deltaGyroAngle.A[YAW]) * EstM.V.Y;    EstM.V.Y =  ssin(deltaGyroAngle.A[YAW]) * EstM.V.X + scos(deltaGyroAngle.A[YAW]) * EstM.V.Y;    // Apply complimentary filter (Gyro drift correction)    for (axis = 0; axis < 3; axis++)      EstM.A[axis] = (EstM.A[axis] * GYR_CMPF_FACTOR + mgSmooth[axis]) / (GYR_CMPF_FACTOR + 1);    // Attitude of the estimated vector      heading = _atan2(EstM.V.Y, EstM.V.X) / 10;  #endif }``

Basically this is second independent filter to combine Gyro with Magnitometer vector. Which is used only to get heading. I do not have HMC5843, so I cannot test it and maybe it is not working properly.. Regards,
ziss_dm

Alexinparis
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

### Re: getEstimatedAttitude: Idea

ziss_dm wrote:Hi Alex,
It is quite simple to implement small inclination calculations, but is it really good idea? Basicaly it would increase control loop timing and decrise system response time for big angles. And PIDs would not be optimal any more. From my expirience: I can tune PIDs for level flight and it really stable even in wind gusts, but as soon I inclinate quad, it start oscilating. I'm not 100% sure that it is due changes in control loop timing, maybe something else wrong, but this is my current expirience. Maybe it is better to make control loop timing as stable as possible?

You are probably right, as long as the extra time spent for low angle is low.
I think something like 0.5ms should be acceptable.
But if the extra time is too long (I've not done the test with the mag) and renders the system less stable, I think it's better to have the best we can get around the center attitude even if it's less stable with high angle.

About magnetometer, could you please explain the basic idea how we want to use it? Do we want it to replace Accelerometer or use it only for Heading or implement combined filter for all 3 sensors?. Without too much thinking, it could be something like this:

The magnetometer is used only to retrieve an absolute orientation, not for a acc substitution.

Basically this is second independent filter to combine Gyro with Magnitometer vector. Which is used only to get heading. I do not have HMC5843, so I cannot test it and maybe it is not working properly.. It's also very interesting because the result is gyro-coupled with a comp filter and it saves hard trigonometric calculations.
I really have to seriously test this approach and measure the different loop times.

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

### Re: getEstimatedAttitude: Idea

Hi,
Slightly updated version:

Code: Select all

``typedef struct {  float X;  float Y;  float Z;} t_int_vector_def;typedef union {  float          A;  t_int_vector_def V;} t_int_vector;#define ACC_LPF_FACTOR  16#define MG_LPF_FACTOR   4#define GYR_CMPF_FACTOR 300.0// Small angle approximation#define ssin(val)  (val)#define scos(val) 1.0#define stan(val) (val)#define squared(val) (val * val)#define GYRO_SCALE (200.0f * PI)/((32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.5f// 1.5 is emperical, not sure what it meansint16_t _atan2( float y, float x ){  #define ONEQTR_PI  M_PI / 4.0  #define THRQTR_PI  3.0 * M_PI / 4.0  float r, angle;  float abs_y = fabs(y) + 1e-10f;      // kludge to prevent 0/0 condition  if ( x < 0.0f ) {    r = (x + abs_y) / (abs_y - x);    angle = THRQTR_PI;  } else {    r = (x - abs_y) / (x + abs_y);    angle = ONEQTR_PI;  }  angle += (0.1963f * r * r - 0.9817f) * r;  angle *= 572.9577951;  return (y < 0.0f) ? -angle : angle;}void getEstimatedAttitude(){  uint8_t axis;  float  AccMag = 0;  static t_int_vector EstV = {0,0,200};  static t_int_vector EstM = {0,200,0};  static t_int_vector deltaGyroAngle;  static uint16_t PreviousTime;  static int16_t mgSmooth;  //projection of smoothed and normalized magnetic vector on x/y/z axis, as measured by magnetometer  uint16_t CurrentTime  = micros();  uint16_t deltaTime = CurrentTime - PreviousTime;  PreviousTime = CurrentTime;  // Initialization  for (axis = 0; axis < 3; axis++) {    // LPF for ACC values    accSmooth[axis] = (accSmooth[axis] * (ACC_LPF_FACTOR - 1) + accADC[axis]) / ACC_LPF_FACTOR;    AccMag += square(accSmooth[axis]);    // Gyro angle change since last time.    deltaGyroAngle.A[axis]  = gyroADC[axis] * (deltaTime * GYRO_SCALE);    #if defined(HMC5843)      mgSmooth[axis] = (mgSmooth[axis] * (MG_LPF_FACTOR - 1) + magADC[axis]) / MG_LPF_FACTOR;    #endif   }  // Rotate Estimated vector, ROLL  EstV.V.Z =  scos(deltaGyroAngle.A[ROLL]) * EstV.V.Z - ssin(deltaGyroAngle.A[ROLL]) * EstV.V.X;  EstV.V.X =  ssin(deltaGyroAngle.A[ROLL]) * EstV.V.Z + scos(deltaGyroAngle.A[ROLL]) * EstV.V.X;  // Rotate Estimated vector, PITCH  EstV.V.Y =  scos(deltaGyroAngle.A[PITCH]) * EstV.V.Y + ssin(deltaGyroAngle.A[PITCH]) * EstV.V.Z;  EstV.V.Z = -ssin(deltaGyroAngle.A[PITCH]) * EstV.V.Y + scos(deltaGyroAngle.A[PITCH]) * EstV.V.Z;  // Apply complimentary filter (Gyro drift correction)  // If accel magnitude >1.4G or <0.6G => we neutralize the effect of accelerometers in the angle estimation.  // To do that, we just skip filter, as EstV already rotated by Gyro  AccMag =  AccMag / square(acc_1G);  if (!(0.36f > AccMag or AccMag > 1.96f)) {    for (axis = 0; axis < 3; axis++)      EstV.A[axis] = (EstV.A[axis] * GYR_CMPF_FACTOR + accSmooth[axis]) / (GYR_CMPF_FACTOR + 1);  }  // Attitude of the estimated vector    angle[ROLL]  =  _atan2(EstV.V.X, EstV.V.Z);  angle[PITCH] =  _atan2(EstV.V.Y, EstV.V.Z);  #if defined(HMC5843)    // Rotate Estimated vector, YAW    EstM.V.X =  scos(deltaGyroAngle.A[YAW]) * EstM.V.X - ssin(deltaGyroAngle.A[YAW]) * EstM.V.Y;    EstM.V.Y =  ssin(deltaGyroAngle.A[YAW]) * EstM.V.X + scos(deltaGyroAngle.A[YAW]) * EstM.V.Y;    // Apply complimentary filter (Gyro drift correction)    for (axis = 0; axis < 3; axis++)      EstM.A[axis] = (EstM.A[axis] * GYR_CMPF_FACTOR + mgSmooth[axis]) / (GYR_CMPF_FACTOR + 1);    // Attitude of the estimated vector      heading = _atan2(EstM.V.X, EstM.V.Y) / 10;  #endif }``

Regards,
ziss_dm

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

### Re: getEstimatedAttitude: Idea (beta testers needed)

Hi,

Looks like the procedure is ready to fly tests. The benefits are:
1) Small footprint (up to 1514 bytes less than original one, depends on configuration)
2) High calculation speeds (2400-2600 cycle time with ACC configuration)
3) Magnetometer support (not tested)
4) User-configurable parameters for fine tuning.
a) LPF for ACC and Magnetometer
b) Gyro weights for CF

Code: Select all

``// **************************************************// Simplified IMU based on "Complementary Filter"// Inspired by http://starlino.com/imu_guide.html//// The following ideas was used in this project:// 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix// 2) Small-angle approximation: http://en.wikipedia.org/wiki/Small-angle_approximation// 3) C. Hastings approximation for atan2()// 4) Optimization tricks: http://www.hackersdelight.org///// Currently Magnetometer uses separate CF which is used only// for heading calculations.//// Last Modified: 05/06/2011// Version: V0.6   // **************************************************//******  advanced users settings *******************/* Set the Low Pass Filter factor for ACC *//* Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time*//* Comment this if  you do not want filter at all.*//* Default WMC value: 8*/#define ACC_LPF_FACTOR 8/* Set the Low Pass Filter factor for Magnetometer *//* Increasing this value would reduce Magnetometer noise (not visible in GUI), but would increase Magnetometer lag time*//* Comment this if  you do not want filter at all.*//* Default WMC value: n/a*/#define MG_LPF_FACTOR 4/* Set the Gyro Weight for Gyro/Acc complementary filter *//* Increasing this value would reduce and delay Acc influence on the output of the filter*//* Default WMC value: 300*/#define GYR_CMPF_FACTOR 300.0f/* Set the Gyro Weight for Gyro/Magnetometer complementary filter *//* Increasing this value would reduce and delay Magnetometer influence on the output of the filter*//* Default WMC value: n/a*/#define GYR_CMPFM_FACTOR 300.0f//****** end of advanced users settings *************#define INV_GYR_CMPF_FACTOR   (1.0f / (GYR_CMPF_FACTOR  + 1.0f))#define INV_GYR_CMPFM_FACTOR  (1.0f / (GYR_CMPFM_FACTOR + 1.0f))#if defined(ITG3200) || defined(L3G4200D)  #define GYRO_SCALE ((200.0f * PI)/((32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.5f)  // 1.5 is emperical, not sure what it means  // should be in rad/sec#else  #define GYRO_SCALE (1.0f/200e6f)  // empirical, depends on WMP on IDG datasheet, tied of deg/ms sensibility  // !!!!should be adjusted to the rad/sec#endif // Small angle approximation#define ssin(val) (val)#define scos(val) 1.0ftypedef struct {  float X;  float Y;  float Z;} t_fp_vector_def;typedef union {  float   A;  t_fp_vector_def V;} t_fp_vector;int16_t _atan2(float y, float x){ #define fp_is_neg(val) ((((byte*)&val) & 0x80) != 0) float z = y / x; int16_t zi = abs(int16_t(z * 100));  int8_t y_neg = fp_is_neg(y); if ( zi < 100 ){   if (zi > 10)      z = z / (1.0f + 0.28f * z * z);   if (fp_is_neg(x)) {     if (y_neg) z -= PI;     else z += PI;   } } else {   z = (PI / 2.0f) - z / (z * z + 0.28f);   if (y_neg) z -= PI; } z *= (180.0f / PI * 10);  return z;}void getEstimatedAttitude(){  uint8_t axis;  int16_t  AccMag = 0;  static t_fp_vector EstV = {0,0,200};  static t_fp_vector EstM = {0,200,0};  float deltaGyroAngle;  static uint16_t PreviousTime;  static int16_t mgSmooth;  //projection of smoothed and normalized magnetic vector on x/y/z axis, as measured by magnetometer  uint16_t CurrentTime  = micros();  float deltaTime = (CurrentTime - PreviousTime) * GYRO_SCALE;  PreviousTime = CurrentTime;  // Initialization  for (axis = 0; axis < 3; axis++) {    #if defined(ACC_LPF_FACTOR)      // LPF for ACC values      accSmooth[axis] = (accSmooth[axis] * (ACC_LPF_FACTOR - 1) + accADC[axis]) / ACC_LPF_FACTOR;      AccMag += (accSmooth[axis] * 10 / acc_1G) * (accSmooth[axis] * 10 / acc_1G);      #define ACC_VALUE accSmooth[axis]    #else        accSmooth[axis] = accADC[axis];      AccMag += (accADC[axis] * 10 / acc_1G) * (accADC[axis] * 10 / acc_1G);      #define ACC_VALUE accADC[axis]    #endif      #if defined(HMC5843) & defined(MG_LPF_FACTOR)      // LPF for Magnetometer values      mgSmooth[axis] = (mgSmooth[axis] * (MG_LPF_FACTOR - 1) + magADC[axis]) / MG_LPF_FACTOR;      #define MAG_VALUE mgSmooth[axis]    #else        #define MAG_VALUE magADC[axis]    #endif   }  // Rotate Estimated vector, ROLL  deltaGyroAngle  = gyroADC[ROLL] * deltaTime;  EstV.V.Z =  scos(deltaGyroAngle) * EstV.V.Z - ssin(deltaGyroAngle) * EstV.V.X;  EstV.V.X =  ssin(deltaGyroAngle) * EstV.V.Z + scos(deltaGyroAngle) * EstV.V.X;  // Rotate Estimated vector, PITCH  deltaGyroAngle  = gyroADC[PITCH] * deltaTime;  EstV.V.Y =  scos(deltaGyroAngle) * EstV.V.Y + ssin(deltaGyroAngle) * EstV.V.Z;  EstV.V.Z = -ssin(deltaGyroAngle) * EstV.V.Y + scos(deltaGyroAngle) * EstV.V.Z;  // Apply complimentary filter (Gyro drift correction)  // If accel magnitude >1.4G or <0.6G => we neutralize the effect of accelerometers in the angle estimation.  // To do that, we just skip filter, as EstV already rotated by Gyro  if (!((36 > AccMag) or (AccMag > 196))) {    for (axis = 0; axis < 3; axis++)      EstV.A[axis] = (EstV.A[axis] * GYR_CMPF_FACTOR + ACC_VALUE) * INV_GYR_CMPF_FACTOR;  }  // Attitude of the estimated vector    angle[ROLL]  =  _atan2(EstV.V.X, EstV.V.Z);  angle[PITCH] =  _atan2(EstV.V.Y, EstV.V.Z);  #if defined(HMC5843)    // Rotate Estimated vector, YAW    deltaGyroAngle  = gyroADC[YAW] * deltaTime;    EstM.V.X =  scos(deltaGyroAngle) * EstM.V.X - ssin(deltaGyroAngle) * EstM.V.Y;    EstM.V.Y =  ssin(deltaGyroAngle) * EstM.V.X + scos(deltaGyroAngle) * EstM.V.Y;    // Apply complimentary filter (Gyro drift correction)    for (axis = 0; axis < 3; axis++)      EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + MAG_VALUE) * INV_GYR_CMPFM_FACTOR;    // Attitude of the estimated vector      heading = _atan2(EstM.V.X, EstM.V.Y) / 10;  #endif }``

Regards,
ziss_dm

Posts: 65
Joined: Wed Mar 23, 2011 8:17 pm

### Re: getEstimatedAttitude: Idea

I just flight tested this code on my mini quad and seems to work pretty well. Much smaller code size, very nicely done. ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

### Re: getEstimatedAttitude: Idea

Would be also interesting to collect statistics about ACC_LPF_FACTOR.

Regards,
ziss_dm

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

### Re: getEstimatedAttitude: Idea

Hi,
Small report after flight tests:

Best results was archived with the following configuration:

Code: Select all

``//#define ACC_LPF_FACTOR 8#define GYR_CMPF_FACTOR 700.0f``

With this parameters, my quad was really stable even with poorly balanced props.

Regards,
ziss_dm

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

### Re: getEstimatedAttitude: Idea

Hi,

jhoexp:
I have tried your modified algorithm, but can't display acc and copter attitude in configurator. Everything is working in regular 1.7preter.
Can you check?

regards,
ziss_dm

Syberian
Posts: 12
Joined: Wed Apr 06, 2011 8:56 am

### Re: getEstimatedAttitude: Idea

ziss_dm

Change:
getEstimatedAttitude();
updateIMU(1); //with I2C or ADC ACC
#else
updateIMU(0); //without ACC
#endif

to:

updateIMU(1); //with I2C or ADC ACC
getEstimatedAttitude();
#else
updateIMU(0); //without ACC
#endif

The gyro data are lost somewhere when ITG3200 was selected.

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

### Re: getEstimatedAttitude: Idea

Hi,
This is another version of this IMU. The flight tests showed, that it is necessary to add YAW rotation to the Estimated vector. Without it "yawing" when quad is not level (30-40deg) produce really strange response. The magnetometer support is still not tested.

Code: Select all

``// **************************************************// Simplified IMU based on "Complementary Filter"// Inspired by http://starlino.com/imu_guide.html//// The following ideas was used in this project:// 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix// 2) Small-angle approximation: http://en.wikipedia.org/wiki/Small-angle_approximation// 3) C. Hastings approximation for atan2()// 4) Optimization tricks: http://www.hackersdelight.org///// Currently Magnetometer uses separate CF which is used only// for heading calculations.//// Last Modified: 13/06/2011// Version: V0.7   // **************************************************//******  advanced users settings *******************/* Set the Low Pass Filter factor for ACC *//* Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time*//* Comment this if  you do not want filter at all.*//* Default WMC value: 8*///#define ACC_LPF_FACTOR 8/* Set the Low Pass Filter factor for Magnetometer *//* Increasing this value would reduce Magnetometer noise (not visible in GUI), but would increase Magnetometer lag time*//* Comment this if  you do not want filter at all.*//* Default WMC value: n/a*/#define MG_LPF_FACTOR 4/* Set the Gyro Weight for Gyro/Acc complementary filter *//* Increasing this value would reduce and delay Acc influence on the output of the filter*//* Default WMC value: 300*/#define GYR_CMPF_FACTOR 310.0f/* Set the Gyro Weight for Gyro/Magnetometer complementary filter *//* Increasing this value would reduce and delay Magnetometer influence on the output of the filter*//* Default WMC value: n/a*/#define GYR_CMPFM_FACTOR 300.0f//****** end of advanced users settings *************#define INV_GYR_CMPF_FACTOR   (1.0f / (GYR_CMPF_FACTOR  + 1.0f))#define INV_GYR_CMPFM_FACTOR  (1.0f / (GYR_CMPFM_FACTOR + 1.0f))#if defined(ITG3200) || defined(L3G4200D)  #define GYRO_SCALE ((200.0f * PI)/((32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.5f)  // 1.5 is emperical, not sure what it means  // should be in rad/sec#else  #define GYRO_SCALE (1.0f/200e6f)  // empirical, depends on WMP on IDG datasheet, tied of deg/ms sensibility  // !!!!should be adjusted to the rad/sec#endif // Small angle approximation#define ssin(val) (val)#define scos(val) 1.0ftypedef struct {  float X;  float Y;  float Z;} t_fp_vector_def;typedef union {  float   A;  t_fp_vector_def V;} t_fp_vector;int16_t _atan2(float y, float x){  #define fp_is_neg(val) ((((byte*)&val) & 0x80) != 0)  float z = y / x;  int16_t zi = abs(int16_t(z * 100));   int8_t y_neg = fp_is_neg(y);  if ( zi < 100 ){    if (zi > 10)      z = z / (1.0f + 0.28f * z * z);   if (fp_is_neg(x)) {     if (y_neg) z -= PI;     else z += PI;   }  } else {   z = (PI / 2.0f) - z / (z * z + 0.28f);   if (y_neg) z -= PI;  }  z *= (180.0f / PI * 10);   return z;}void getEstimatedAttitude(){  uint8_t axis;  int16_t  AccMag = 0;  static t_fp_vector GEstV = {0,0,200};  t_fp_vector EstV = GEstV;  static t_fp_vector EstM = {0,200,0};  float deltaGyroAngle;  static uint16_t PreviousTime;  static int16_t mgSmooth;  //projection of smoothed and normalized magnetic vector on x/y/z axis, as measured by magnetometer  uint16_t CurrentTime  = micros();  float deltaTime = (CurrentTime - PreviousTime) * GYRO_SCALE;  PreviousTime = CurrentTime;  // Initialization  for (axis = 0; axis < 3; axis++) {    #if defined(ACC_LPF_FACTOR)      // LPF for ACC values      accSmooth[axis] = (accSmooth[axis] * (ACC_LPF_FACTOR - 1) + accADC[axis]) / ACC_LPF_FACTOR;      AccMag += (accSmooth[axis] * 10 / acc_1G) * (accSmooth[axis] * 10 / acc_1G);      #define ACC_VALUE accSmooth[axis]    #else        accSmooth[axis] = accADC[axis];      AccMag += (accADC[axis] * 10 / acc_1G) * (accADC[axis] * 10 / acc_1G);      #define ACC_VALUE accADC[axis]    #endif      #if defined(HMC5843) & defined(MG_LPF_FACTOR)      // LPF for Magnetometer values      mgSmooth[axis] = (mgSmooth[axis] * (MG_LPF_FACTOR - 1) + magADC[axis]) / MG_LPF_FACTOR;      #define MAG_VALUE mgSmooth[axis]    #else        #define MAG_VALUE magADC[axis]    #endif   }  // Rotate Estimated vector, ROLL  deltaGyroAngle  = gyroADC[ROLL] * deltaTime;  EstV.V.Z =  scos(deltaGyroAngle) * EstV.V.Z - ssin(deltaGyroAngle) * EstV.V.X;  EstV.V.X =  ssin(deltaGyroAngle) * EstV.V.Z + scos(deltaGyroAngle) * EstV.V.X;  // Rotate Estimated vector, PITCH  deltaGyroAngle  = gyroADC[PITCH] * deltaTime;  EstV.V.Y =  scos(deltaGyroAngle) * EstV.V.Y + ssin(deltaGyroAngle) * EstV.V.Z;  EstV.V.Z = -ssin(deltaGyroAngle) * EstV.V.Y + scos(deltaGyroAngle) * EstV.V.Z;  // Rotate Estimated vector, YAW  deltaGyroAngle  = gyroADC[YAW] * deltaTime;  EstV.V.X =  scos(deltaGyroAngle) * EstV.V.X - ssin(deltaGyroAngle) * EstV.V.Y;  EstV.V.Y =  ssin(deltaGyroAngle) * EstV.V.X + scos(deltaGyroAngle) * EstV.V.Y;  // Apply complimentary filter (Gyro drift correction)  // If accel magnitude >1.4G or <0.6G => we neutralize the effect of accelerometers in the angle estimation.  // To do that, we just skip filter, as EstV already rotated by Gyro  if (!((36 > AccMag) or (AccMag > 196))) {    for (axis = 0; axis < 3; axis++)      EstV.A[axis] = (EstV.A[axis] * GYR_CMPF_FACTOR + ACC_VALUE) * INV_GYR_CMPF_FACTOR;  }  // Attitude of the estimated vector    angle[ROLL]  =  _atan2(EstV.V.X, EstV.V.Z);  angle[PITCH] =  _atan2(EstV.V.Y, EstV.V.Z);  GEstV = EstV;  #if defined(HMC5843)    // Rotate Estimated vector, YAW    deltaGyroAngle  = gyroADC[YAW] * deltaTime;    EstM.V.X =  scos(deltaGyroAngle) * EstM.V.X - ssin(deltaGyroAngle) * EstM.V.Y;    EstM.V.Y =  ssin(deltaGyroAngle) * EstM.V.X + scos(deltaGyroAngle) * EstM.V.Y;    // Apply complimentary filter (Gyro drift correction)    for (axis = 0; axis < 3; axis++)      EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + MAG_VALUE) * INV_GYR_CMPFM_FACTOR;    // Attitude of the estimated vector      heading = _atan2(EstM.V.X, EstM.V.Y) / 10;  #endif }``

Regards,
ziss_dm

Alexinparis
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

### Re: getEstimatedAttitude: Idea

Hi,

No more than 200ms from the current small angle approximation, I like it !

But... there is something wrong with the MAG.
Even for a 10 degrees inclination, the mag indication can't be trusted.
I know it's not easy to adjust as you don't have a MAG to test it.

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

### Re: getEstimatedAttitude: Idea

Hi,
Looks like I found problem. I thought that magnetic field vector is parallel to the ground and pointing north... But reality is it is pointing up and slightly inclinated to the north (about 15 deg). That is why atan2(y,x) was giving strange result as it too close to singularity. To avoid this we can create cross product vector between gravity vector and magnetic field vector. This vector would be parallel to the ground and would point 90 deg from north. This vector is more suitable for atan2(). So this is a result:

Code: Select all

``// **************************************************// Simplified IMU based on "Complementary Filter"// Inspired by http://starlino.com/imu_guide.html//// The following ideas was used in this project:// 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix// 2) Small-angle approximation: http://en.wikipedia.org/wiki/Small-angle_approximation// 3) C. Hastings approximation for atan2()// 4) Optimization tricks: http://www.hackersdelight.org///// Currently Magnetometer uses separate CF which is used only// for heading approximation.//// Last Modified: 19/06/2011// Version: V1.1   // **************************************************//******  advanced users settings *******************/* Set the Low Pass Filter factor for ACC *//* Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time*//* Comment this if  you do not want filter at all.*//* Default WMC value: 8*/#define ACC_LPF_FACTOR 8/* Set the Low Pass Filter factor for Magnetometer *//* Increasing this value would reduce Magnetometer noise (not visible in GUI), but would increase Magnetometer lag time*//* Comment this if  you do not want filter at all.*//* Default WMC value: n/a*///#define MG_LPF_FACTOR 4/* Set the Gyro Weight for Gyro/Acc complementary filter *//* Increasing this value would reduce and delay Acc influence on the output of the filter*//* Default WMC value: 300*/#define GYR_CMPF_FACTOR 310.0f/* Set the Gyro Weight for Gyro/Magnetometer complementary filter *//* Increasing this value would reduce and delay Magnetometer influence on the output of the filter*//* Default WMC value: n/a*/#define GYR_CMPFM_FACTOR 200.0f//****** end of advanced users settings *************#define INV_GYR_CMPF_FACTOR   (1.0f / (GYR_CMPF_FACTOR  + 1.0f))#define INV_GYR_CMPFM_FACTOR  (1.0f / (GYR_CMPFM_FACTOR + 1.0f))#if defined(ITG3200) || defined(L3G4200D)  //#define GYRO_SCALE ((2000.0f * PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.155f)    // +-2000/sec deg scale  #define GYRO_SCALE ((200.0f * PI)/((32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.5f)       // +- 200/sec deg scale  // 1.5 is emperical, not sure what it means  // should be in rad/sec#else  #define GYRO_SCALE (1.0f/200e6f)  // empirical, depends on WMP on IDG datasheet, tied of deg/ms sensibility  // !!!!should be adjusted to the rad/sec#endif // Small angle approximation#define ssin(val) (val)#define scos(val) 1.0ftypedef struct {  float X;  float Y;  float Z;} t_fp_vector_def;typedef union {  float   A;  t_fp_vector_def V;} t_fp_vector;int16_t _atan2(float y, float x){  #define fp_is_neg(val) ((((byte*)&val) & 0x80) != 0)  float z = y / x;  int16_t zi = abs(int16_t(z * 100));   int8_t y_neg = fp_is_neg(y);  if ( zi < 100 ){    if (zi > 10)      z = z / (1.0f + 0.28f * z * z);   if (fp_is_neg(x)) {     if (y_neg) z -= PI;     else z += PI;   }  } else {   z = (PI / 2.0f) - z / (z * z + 0.28f);   if (y_neg) z -= PI;  }  z *= (180.0f / PI * 10);   return z;}void getEstimatedAttitude(){  uint8_t axis;  int16_t  AccMag = 0;  static t_fp_vector GEstG = {0,0,200};  t_fp_vector EstG = GEstG;  static t_fp_vector EstM = {10,10,200};  float deltaGyroAngle;  static uint16_t PreviousTime;  static int16_t mgSmooth;  //projection of smoothed and normalized magnetic vector on x/y/z axis, as measured by magnetometer  uint16_t CurrentTime  = micros();  float deltaTime = (CurrentTime - PreviousTime) * GYRO_SCALE;  PreviousTime = CurrentTime;  // Initialization  for (axis = 0; axis < 3; axis++) {    #if defined(ACC_LPF_FACTOR)      // LPF for ACC values      accSmooth[axis] = (accSmooth[axis] * (ACC_LPF_FACTOR - 1) + accADC[axis]) / ACC_LPF_FACTOR;      AccMag += (accSmooth[axis] * 10 / acc_1G) * (accSmooth[axis] * 10 / acc_1G);      #define ACC_VALUE accSmooth[axis]    #else        accSmooth[axis] = accADC[axis];      AccMag += (accADC[axis] * 10 / acc_1G) * (accADC[axis] * 10 / acc_1G);      #define ACC_VALUE accADC[axis]    #endif      #if defined(HMC5843) & defined(MG_LPF_FACTOR)      // LPF for Magnetometer values      mgSmooth[axis] = (mgSmooth[axis] * (MG_LPF_FACTOR - 1) + magADC[axis]) / MG_LPF_FACTOR;      #define MAG_VALUE mgSmooth[axis]    #else        #define MAG_VALUE magADC[axis]    #endif   }  // Rotate Estimated vector(s), ROLL  deltaGyroAngle  = gyroADC[ROLL] * deltaTime;  EstG.V.Z =  scos(deltaGyroAngle) * EstG.V.Z - ssin(deltaGyroAngle) * EstG.V.X;  EstG.V.X =  ssin(deltaGyroAngle) * EstG.V.Z + scos(deltaGyroAngle) * EstG.V.X;  #if defined(HMC5843)    EstM.V.Z =  scos(deltaGyroAngle) * EstM.V.Z - ssin(deltaGyroAngle) * EstM.V.X;    EstM.V.X =  ssin(deltaGyroAngle) * EstM.V.Z + scos(deltaGyroAngle) * EstM.V.X;  #endif   // Rotate Estimated vector(s), PITCH  deltaGyroAngle  = gyroADC[PITCH] * deltaTime;  EstG.V.Y =  scos(deltaGyroAngle) * EstG.V.Y + ssin(deltaGyroAngle) * EstG.V.Z;  EstG.V.Z = -ssin(deltaGyroAngle) * EstG.V.Y + scos(deltaGyroAngle) * EstG.V.Z;  #if defined(HMC5843)    EstM.V.Y =  scos(deltaGyroAngle) * EstM.V.Y + ssin(deltaGyroAngle) * EstM.V.Z;    EstM.V.Z = -ssin(deltaGyroAngle) * EstM.V.Y + scos(deltaGyroAngle) * EstM.V.Z;  #endif   // Rotate Estimated vector(s), YAW  deltaGyroAngle  = gyroADC[YAW] * deltaTime;  EstG.V.X =  scos(deltaGyroAngle) * EstG.V.X - ssin(deltaGyroAngle) * EstG.V.Y;  EstG.V.Y =  ssin(deltaGyroAngle) * EstG.V.X + scos(deltaGyroAngle) * EstG.V.Y;  #if defined(HMC5843)    EstM.V.X =  scos(deltaGyroAngle) * EstM.V.X - ssin(deltaGyroAngle) * EstM.V.Y;    EstM.V.Y =  ssin(deltaGyroAngle) * EstM.V.X + scos(deltaGyroAngle) * EstM.V.Y;  #endif   // Apply complimentary filter (Gyro drift correction)  // If accel magnitude >1.4G or <0.6G => we neutralize the effect of accelerometers in the angle estimation.  // To do that, we just skip filter, as EstV already rotated by Gyro  if (!((36 > AccMag) or (AccMag > 196))) {    for (axis = 0; axis < 3; axis++)      EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + ACC_VALUE) * INV_GYR_CMPF_FACTOR;  }  // Attitude of the estimated vector    angle[ROLL]  =  _atan2(EstG.V.X, EstG.V.Z);  angle[PITCH] =  _atan2(EstG.V.Y, EstG.V.Z);  GEstG = EstG;  #if defined(HMC5843)    // Apply complimentary filter (Gyro drift correction)    for (axis = 0; axis < 3; axis++)      EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + MAG_VALUE) * INV_GYR_CMPFM_FACTOR;    // Attitude of the cross product vector GxM    heading = _atan2(EstG.V.Z * EstM.V.X - EstG.V.X * EstM.V.Z, EstG.V.Y * EstM.V.Z - EstG.V.Z * EstM.V.Y) / 10;  #endif }``

But to make it work it is necessary to "align" magnetometer to ACC, especially Z axis. (For some reason, by default it is inverted). I can post my sensor orientation for reference. Additionally we can further optimize this, for example we can estimate heading only when we have new data from Mag, etc.

regards,
ziss_dm

Alexinparis
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

### Re: getEstimatedAttitude: Idea

you're a genious !
briefly tested and everything works as expected

I take it for the next version ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

### Re: getEstimatedAttitude: Idea

Hi,

We need to invent cool name for algorithm. Some examples:
VRS - Vector reference system
SAHRS - Simple Attitude and Heading Reference System
TAHRS - Tiny Attitude and Heading Reference System

regards,
ziss_dm

Alexinparis
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

### Re: getEstimatedAttitude: Idea

I like the last 2 names, but it's up to you as it's your creation I think about one thing we could maybe improve: ACC neutralization.
The most difficult part is to neutralize the ACC at the right time.
The current implementation is very basic, if the ACC vector is too high or too low we just discard it from the calculation.
It doesn't work well in turn, because the centrifugal force is not so high and disturb a lot the estimation.
So the question is how to exclude this force from the attitude estimator?
One easy workaround is to increase GYR_CMPF_FACTOR thinking about the temporary effect of this force during a flight, but don't like this option as if decrease the convergence toward the perfect angle.

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

### Re: getEstimatedAttitude: Idea

Hi Alex,

I think, that most erratic behavior in banked turns was cased not by changed acc vector, but by the fact that we missed rotation about Z axis. Last weekend (added rotation and changed ITG3200 init) I was doing nice banked turns 30-40deg inclination and feeling was really good: you roll in and then pretty much just holding elevator. You release sticks and it slowly leveling up. So now I think centrifugal force is actually helping pilot and was even thinking even to remove "fork" of valid ACC values. But this is only first impression. It would be nice to collect flight expirience, and then we could progressivelly eliminate unwanted behavior.

regards,
ziss_dm

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

### Re: getEstimatedAttitude: Idea

http://wbb.multiwii.com/viewtopic.php?f=8&t=391
probably this post should be put here... in any case could you look at this issue? maybe not an issue but feature?! AlainB
Posts: 4
Joined: Wed Jun 15, 2011 8:46 pm

### Re: getEstimatedAttitude: Idea

Hello,
As a newbee on this project, this is a kick off post...
As suggested by Alex, I started investigating the Altitude Hold problem... a real challenge !
I wrote a short Sketch in order to see what we can really expect from the BMA085 with "against without "easing filtering technics. I' certainly try to compare with other Baros (the ones on the MKs are working great, but with analog output !)

I am not used to posting on a forum : the attachment of the picture is probably ok, but I am not sure I succeded in attaching the Processing Sketch file "Baro_Test" ??? if not so how can somebody tell me how to do ?

Alain

AlainB
Posts: 4
Joined: Wed Jun 15, 2011 8:46 pm

### Re: getEstimatedAttitude: Idea

I've put my foot in it ! the previous post should be shifted to "Alt Hold Ideas and discussion"
sorry

re: the code...

Code: Select all

``/** * Baro_Test  * by AlainB          rel. 17 06 15 *  * Sends 'Z' out the serial port ... * listens for bytes received, and displays their value.  * This is just a quick application for MultiWii data analysis  */import processing.serial.*; // serial librarySerial g_serial;int windowsX    = 400; int windowsY    = 600;float time1, time2;  int p =0, index=0, index1=0;  int throttle, altSmooth, altZero, altHold, accSmooth, carZ ;  float easing = 0.05, altSoft ;  int alt_Soft;byte[] inBuf = new byte;int read16() {return (inBuf[p++]&0xff) + (inBuf[p++]<<8);}int read8()  {return inBuf[p++]&0xff;}void setup() {  size(windowsX,windowsY);  frameRate(15);  println(Serial.list());   String arduinoPort = Serial.list();  g_serial = new Serial(this, arduinoPort , 115200);    background(80);  } void draw() {  time1 = millis();  text("PRESS 'z' :", 20, 20);  if (g_serial.available()>20) g_serial.clear();  while (g_serial.available()>12) processSerialData();  if ((time1-time2)>100 && g_serial.available()<5) {    g_serial.write('z');    time2 = time1 ;  }    if (index > 0 && index < 32) {    if (index ==1) background(80);    text (throttle + "         Smooth= "+altSmooth+"          Soft= "+alt_Soft+"         acc= "+accSmooth, 20,35+(index*15));   index++;  }  else {if (index1==1) index = 1;         else index =0;       }         }void processSerialData() {    int frame_size = 12;  if (g_serial.read() == 'Z') {    g_serial.readBytes(inBuf);    if (inBuf[frame_size-1]=='Z') {      text("trottle           altSmooth          alt_Soft         accSmooth", 20, 550);      p=1;        throttle   = read16();      altSmooth = read16();      altZero   = read16();      altHold   = read16();      accSmooth = read16();      carZ      = read8();                altSoft += (float (altSmooth) - altSoft) * easing ;      alt_Soft = int (altSoft);    }    }   }void keyPressed() {  // Send the keystroke out:  if (key == 'z') { index1 =1; }  if (key == 'Z') { index1 =0; }} ``

regards
Alain

AlainB
Posts: 4
Joined: Wed Jun 15, 2011 8:46 pm

### Re: getEstimatedAttitude: Idea

Posting code... it seems OK now
but, the Ardu code in the copter needs to send something when it receives "Z" from the PC : this should be included in "Serial"

Code: Select all

``    case 'z':  // arduino to PC for data analysis      point=0;      serialize8('Z');      serialize16(rcCommand);      serialize16(altitudeSmooth);      serialize16(altitudeZero);      serialize16(altitudeHold);      serialize16(accSmooth);            serialize8('Z');      UartSendData();      break;``

Alain

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

### Re: getEstimatedAttitude: Idea

Hi Alex,

In the revision 249 the CF for MAG was changed to this:

Code: Select all

``    // Apply complimentary filter (Gyro drift correction)    for (axis = 0; axis < 3; axis++)      EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR - MAG_VALUE) * INV_GYR_CMPFM_FACTOR;``

Are you sure this is good idea? Now it is quite confusing.. regards,
ziss_dm

Alexinparis
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

### Re: getEstimatedAttitude: Idea

Hi,

It's a way to invert the MAG arrow direction, pointing now to the magnetic north (approximatively).
I did this to set up common basis to develop latter GPS.
Do you see other board effects ?

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

### Re: getEstimatedAttitude: Idea

Hi,

Just lot of confusion :
1) Just strange to see "-" in CF. Maybe it is better to change order in cross product. 2) This is 180 degree rotation
3) As I remember, with properly aligned ACC and MAG it should point West, so we need 90 degree rotation.
4) HMC5883 already rotated 90 degree in sensors.pde

Code: Select all

``  #if defined (HMC5883)    MAG_ORIENTATION( ((rawADC<<8) | rawADC) ,                    -((rawADC<<8) | rawADC) ,                    -((rawADC<<8) | rawADC) );  #endif``

X,Y swapped and chaged sign, but looks like in wrong direction. 5) If I follow FAQ instructions about aligment, it is pointing north without "-". With "-" it is pointing south.

Just too many minuses.... ;(

regards,
ziss_dm

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

### Re: getEstimatedAttitude: Idea

Hi Alex,

I was always wandering why WMC level mode has kind of "sticky" feeling.. And lso why ACC values never return to "original" in the GUI in case you tilted model after calibration and return it back.. Looks like I have found little problem with our LPF:

Code: Select all

``accSmooth[axis] = (accSmooth[axis] * (ACC_LPF_FACTOR - 1) + accADC[axis]) / ACC_LPF_FACTOR;``

or

Code: Select all

``accSmooth[axis] =(accSmooth[axis]*15+accADC[axis]+8)>>4;``

The problem here, that we are dropping bits and accumulating round-off errors. If you run numerical simulation, for step response 0-100, for example, the first one would stabilize at 93 (7% error). Second one, would stabilize at 101 (probably +7 would be more apropriate). This is looks better unless you simulate step response 100-0 ;( First one would stabilize at 0, the second one at 8 (8% error)!!!!
To avoid switching to FP or designing complex IIR filters, I would probably suggest slightly rewrite it:

Code: Select all

``      accSmooth[axis] = (accSmooth[axis] * (ACC_LPF_FACTOR - 1) / ACC_LPF_FACTOR + accADC[axis]); // LPF for ACC values``

This way accSmooth[axis] accumulates values multiplied by ACC_LPF_FACTOR but without loosing precision. And because algorithm does not care aboult length of the vector (currently I have commented allowed acc magnitude) it works quite well. regards,
ziss_dm

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

### Re: getEstimatedAttitude: Idea

Hi ziss_dm,

About old LPF implementation. As I found it "skip" small ACC value changes (where for big ACC_LPF_FACTOR, e.g. 32, it "skip" whole degrees), because of the dropping bits in cycle, BUT it really helps to filter vibrations, because here small changes of the ACC values are vibrations as usual. So it seems this LPF bug is feature for us About your implementation. Yes it has more accuracy but result will be much more than input values. E.g. input=5, ACC_LPF_FACTOR=5... result will be = 21

Also I have described a little bit advanced LPF to know the cut frequency (based on dt). It takes not so much cycle time:
viewtopic.php?f=8&t=671&start=60#p4349

What do think?

thx-
Alex

mr.rc-cam
Posts: 457
Joined: Wed Jul 27, 2011 11:36 pm

### Re: getEstimatedAttitude: Idea

ziss_dm wrote:To avoid switching to FP or designing complex IIR filters, I would probably suggest slightly rewrite it:

Code: Select all

``      accSmooth[axis] = (accSmooth[axis] * (ACC_LPF_FACTOR - 1) / ACC_LPF_FACTOR + accADC[axis]); // LPF for ACC values``

Are you sure you didn't mean this? :

Code: Select all

``accSmooth[axis] = (((accSmooth[axis] * (ACC_LPF_FACTOR - 1)) / ACC_LPF_FACTOR) + (accADC[axis]/ACC_LPF_FACTOR)); // LPF for ACC values``

- Thomas

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

### Re: getEstimatedAttitude: Idea

Hi Alex,

About old LPF implementation. As I found it "skip" small ACC value changes (where for big ACC_LPF_FACTOR, e.g. 32, it "skip" whole degrees), because of the dropping bits in cycle, BUT it really helps to filter vibrations, because here small changes of the ACC values are vibrations as usual. So it seems this LPF bug is feature for us

But it should skip FAST changes. With constant input 100 it must produce constant output 100 suner or later.. Not sure about this feature, as it produces additional noise:
IIR roundoff noise
If you really want to reduce noise by expence of resolution it is better to just to reduce resolution. About your implementation. Yes it has more accuracy but result will be much more than input values. E.g. input=5, ACC_LPF_FACTOR=5... result will be = 21

Yes, but it does not matter. Additionally it has kind of oversamplig effect. Also I have described a little bit advanced LPF to know the cut frequency (based on dt). It takes not so much cycle time:

It would make sense in case we know filter characteristics we need. Currently it is try-error process, from this point of view it does not really matter that you adjusting some "factor" or "cut-off freq". And you can always calculate one from another regards,
ziss_dm

Alexinparis
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

### Re: getEstimatedAttitude: Idea

ziss_dm wrote:To avoid switching to FP or designing complex IIR filters, I would probably suggest slightly rewrite it:

Code: Select all

``      accSmooth[axis] = (accSmooth[axis] * (ACC_LPF_FACTOR - 1) / ACC_LPF_FACTOR + accADC[axis]); // LPF for ACC values``

This way accSmooth[axis] accumulates values multiplied by ACC_LPF_FACTOR but without loosing precision. And because algorithm does not care aboult length of the vector (currently I have commented allowed acc magnitude) it works quite well. Hi,
You're right, the current LPF could even explain some angle inaccuracy. This is something we must change.
I like this approach, but it has one drawback: it could overflow rapidly an int16_t value.
accSmooth will reach ACC_LPF_FACTOR*accADC value for constant output
ACC_LPF_FACTOR*(ACC_LPF_FACTOR - 1) is 56 for a factor 8
With a resolution of 2G, the BMA180 could output up to 1024 => 56*1024 is beyond the max of 32768 for a signed int16.
I wouldn't like switching to an int32_t as it's almost 100us delay worse.

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

### Re: getEstimatedAttitude: Idea

Hi,
You are right.. ;( But I was thinking, do we really need LPF? The CF is LPF by it's nature, so I have tried to remove pre-filtering and increase CF factor to 600 and looks like I have pretty much same behavior. It has slightly different reaction on really sharp stick input, but for my opinion it is even better. Can you try and tel me your impression? regards,
ziss_dm

Alexinparis
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

### Re: getEstimatedAttitude: Idea

Hi,

I will try higher CF factor.
But at 600, the convergence time would be higher, and the gyro has to be perfectly tuned (GYRO_SCALE).
This is what I had in mind in replacement for the LPF:

Code: Select all

``  static uint8_t index,ind;  static int16_t accTemp;  static int16_t sum;  sum[axis] -= accTemp[axis][index%16];  sum[axis] += accADC[axis];  accTemp[axis][index++%16] = accADC[axis];  accSmooth[axis] = sum[axis]/16;``

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

### Re: getEstimatedAttitude: Idea

Hi,

Looks good, only one thing it requires 102 bytes of sram. ;( Do we know how much is left?

Another possibility would be:

Code: Select all

``accSmooth[axis] = (accSmooth[axis] - accSmooth[axis] / ACC_LPF_FACTOR) + accADC[axis]);``

regards,
ziss_dm

Alexinparis
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

### Re: getEstimatedAttitude: Idea

ziss_dm wrote:Hi,

Looks good, only one thing it requires 102 bytes of sram. ;( Do we know how much is left?

Another possibility would be:

Code: Select all

``accSmooth[axis] = (accSmooth[axis] - accSmooth[axis] / ACC_LPF_FACTOR) + accADC[axis]);``

regards,
ziss_dm

It's probably the most elegant way.
adopted mr.rc-cam
Posts: 457
Joined: Wed Jul 27, 2011 11:36 pm

### Re: getEstimatedAttitude: Idea

Alexinparis wrote:
ziss_dm wrote:Another possibility would be:

Code: Select all

``accSmooth[axis] = (accSmooth[axis] - accSmooth[axis] / ACC_LPF_FACTOR) + accADC[axis]);``

It's probably the most elegant way.
adopted I apologize for the intrusion, but I need your help understanding the proposed code. The weighting on the accADC[axis] seems too strong. Are you sure you don't mean:
accSmooth[axis] = (accSmooth[axis] - (accSmooth[axis]/ACC_LPF_FACTOR)) + (accADC[axis]/ACC_LPF_FACTOR);

Also, I think the resolution of the filter's data will be reduced. The existing filter increments in steps of ±1 but the revised code will probably increment by a larger step size that is related to the ACC_LPF_FACTOR value.

Have you installed the new filter code and carefully observed the data values in the GUI? I'll certainly do this later tonight to see what I get.

ziss_dm
Posts: 529
Joined: Tue Mar 08, 2011 5:26 am

### Re: getEstimatedAttitude: Idea

Hi,

The original version:
Af = (Af*7 + An)/8

Ok, now we want Af to be 8 times bigger:
Af = Af*7/8 + An

And we can transform it to:
Af = (Af - Af/8) + An

Also, I think the resolution of the filter's data will be reduced. The existing filter increments in steps of ±1 but the revised code will probably increment by a larger step size that is related to the ACC_LPF_FACTOR value.

Not sure I understand the point. It cannot be worse as we not dropping bits from integrated value, and if you rin simulation 0-100 it looks better:

Code: Select all

``Step   Old  New    FP   1    12  100 12.50   2    23  188 23.44   3    32  265 33.01   4    40  332 41.38   5    47  391 48.71   6    53  443 55.12   7    58  488 60.73   8    63  527 65.64   9    67  562 69.93  10    71  592 73.69  11    74  618 76.98  12    77  641 79.86  13    79  661 82.38  14    81  679 84.58  15    83  695 86.51  16    85  709 88.19  17    86  721 89.67  18    87  731 90.96  19    88  740 92.09  20    89  748 93.08  21    90  755 93.94  22    91  761 94.70  23    92  766 95.36  24    93  771 95.94  25    93  775 96.45  26    93  779 96.89  27    93  782 97.28  28    93  785 97.62  29    93  787 97.92  30    93  789 98.18  31    93  791 98.41  32    93  793 98.61  33    93  794 98.78  34    93  795 98.93  35    93  796 99.07  36    93  797 99.18  37    93  798 99.29  38    93  799 99.37  39    93  800 99.45  40    93  800 99.52  41    93  800 99.58  42    93  800 99.63  43    93  800 99.68  44    93  800 99.72  45    93  800 99.75  46    93  800 99.79  47    93  800 99.81  48    93  800 99.84  49    93  800 99.86  50    93  800 99.87  51    93  800 99.89  52    93  800 99.90  53    93  800 99.92  54    93  800 99.93  55    93  800 99.94  56    93  800 99.94  57    93  800 99.95  58    93  800 99.96  59    93  800 99.96  60    93  800 99.97  61    93  800 99.97  62    93  800 99.97  63    93  800 99.98  64    93  800 99.98  65    93  800 99.98  66    93  800 99.99  67    93  800 99.99  68    93  800 99.99  69    93  800 99.99  70    93  800 99.99  71    93  800 99.99  72    93  800 99.99  73    93  800 99.99  74    93  800 99.99``

regards,
ziss_dm

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

### Re: getEstimatedAttitude: Idea

Hi Dmitry,

It seems you forgot to mention the delimiter for smooth result:
At = At - At/8 + An; // where At is A(temp)
Af = At/8;

thx-
Alex

mr.rc-cam
Posts: 457
Joined: Wed Jul 27, 2011 11:36 pm

### Re: getEstimatedAttitude: Idea

The original version:
Af = (Af*7 + An)/8

Agreed, that is essentially what I see. Specifically my v1.8p2 code version has
Af = (Af*7 + (An+4))/8

Ok, now we want Af to be 8 times bigger:
Af = Af*7/8 + An

This is where I am confused. That will add the full ADC value to 7/8 of the smooth value and the resulting smooth value will be nearly 2X larger. This seems odd to me which is why I thought you meant this:
Af = (Af*7/8) + (An/8)

But enough of my confused guesswork. This morning I installed the new filter in imu.pde. I replaced the old one:

With the new one:
accSmooth[axis] = (accSmooth[axis] - accSmooth[axis] / ACC_LPF_FACTOR) + accADC[axis];

Now in the GUI the Z axis is strange. After calibration it stays at 1000 for nearly all angles (-1000 while inverted). The pitch and roll axis have reduced resolution; What I mean is that the data's smallest step size is about 7. They easily get pegged to ±1000 if the model's angle is more than a few degrees from a level position. With the old filter the data was limited by ±512, so the new filter's data appears to be about 2X too large.

I suspect that I am doing something wrong. I'm using the BMA180. The ACC_LPF_FACTOR is 8. What other changes do I need to make to use the new filter?

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

### Re: getEstimatedAttitude: Idea

Hi mr.rc-cam,

The solution with new filter already promoted by Alex: pls. see here the changes http://code.google.com/p/multiwii/sourc ... de&old=311

the new filter is:

Code: Select all

``accTemp[axis] = (accTemp[axis] - (accTemp[axis] >>4)) + accADC[axis];accSmooth[axis] = accTemp[axis]>>4;``

mr.rc-cam
Posts: 457
Joined: Wed Jul 27, 2011 11:36 pm

### Re: getEstimatedAttitude: Idea

The solution with new filter already promoted by Alex: pls. see here the changes http://code.google.com/p/multiwii/sourc ... de&old=311

Thanks, I'll dig through it to see why I am confused. Edit/update:
the new filter is:

Code: Select all

``accTemp[axis] = (accTemp[axis] - (accTemp[axis] >>4)) + accADC[axis];accSmooth[axis] = accTemp[axis]>>4;``

Very good, thanks for the hand holding!

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

### Re: getEstimatedAttitude: Idea

Hi guys,

It seems with new MultiWii_dev_20111029 it has an drift issue again. So as I supposed before we don't need too accurate LPF for ACC, because it seems that dropping bits in cycle really helps to filter vibrations... this is because that filter implementation "skip" small input values (angles) in spite of frequency and IMU will calculate the angles based only on gyro values (in case of small changes)... and give the correct small angle changes for PID controller... particularly for "I" stabilization parameter...... viewtopic.php?f=8&t=198&start=20#p4791

I also tried to increase LPF factor to 32 (>>5) but dfift is still there in diff with MultiWii_dev_20111017.

Code: Select all

``accTemp[axis] = (accTemp[axis] - (accTemp[axis]>>5)) + accADC[axis];accSmooth[axis] = accTemp[axis]>>5;``

Probably it was windy this weekend, but in diff with MultiWii_dev_20111017 the last DEV was more unstable. All PID params was the same during the tests... So we need to test this one more time when the weather will come more smooth...

Konk
Posts: 2
Joined: Tue May 13, 2014 3:05 am

### Re: getEstimatedAttitude: Idea

I have a question regarding the used comp. filter in 2.3 multiwii imu.cpp:

Code: Select all

``EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + imu.accSmooth[axis]) * INV_GYR_CMPF_FACTOR;``

It seems like the estimated (gyro) Vector (which is float -1 to +1 ?) is just mixed up with the uncompensated/unscaled acceleration value of the axis.

I am sure there is quite some trial and error and thinking behind that formula as it seems to work and does the job with a real low amount of cycles.
However, I don't understand how that mixup can give a useful value.

Plüschi
Posts: 433
Joined: Thu Feb 21, 2013 6:09 am

### Re: getEstimatedAttitude: Idea

EstG is not Gyro but Gravity.

The size of EstG doesent matter, only the direction matters. EstG will start 0,0,0 at startup and then slowly become the size of imu.accsmooth

Konk
Posts: 2
Joined: Tue May 13, 2014 3:05 am

### Re: getEstimatedAttitude: Idea

aaah that makes quite some sense thanks for the hint

rnewbie
Posts: 2
Joined: Fri Jul 17, 2015 12:55 pm

### Re: getEstimatedAttitude: Idea

Hi all:

I am trying to trace the source code of MultiWii (v2.4),
I can't understand the code of current rotateV32 function,

I think now I understand the original rotateV32 function after I read these discussion,
it's based on basic rotation method and been simplified by using small angle approximation,
but how about current function? Is it still based on same method?
I guess it's also been simplified by using small angle approximation, but I don't understand how we rotate the matrix(sin (a+b), cos(a + b)) ... in current function. By Wiki

Code: Select all

``// Rotate Estimated vector(s) with small angle approximation, according to the gyro datavoid rotateV32( t_int32_t_vector *v,int16_t* delta) {  int16_t X = v->V16.X;  int16_t Y = v->V16.Y;  int16_t Z = v->V16.Z;  v->V32.Z -=  mul(delta[ROLL]  ,  X)  + mul(delta[PITCH] , Y);         <------- I don't understand how do we rotate the matrix, is it related to euler angles? or ....  v->V32.X +=  mul(delta[ROLL]  ,  Z)  - mul(delta[YAW]   , Y);  v->V32.Y +=  mul(delta[PITCH] ,  Z)  + mul(delta[YAW]   , X);}``

Sorry for my bad English, and please forgive me if this is a stupid question.

Thanks.

rnewbie
Posts: 2
Joined: Fri Jul 17, 2015 12:55 pm

### Re: getEstimatedAttitude: Idea

Oh, I think I already figure out what it is,
It's related to angle and gravity,

like this
http://www.studyphysics.ca/newnotes/20/ ... sson25.htm

Hope I am right.

Thanks

ThanhNguyen
Posts: 1
Joined: Sun Nov 24, 2019 2:32 pm

### Re: getEstimatedAttitude: Idea

Hello ziss_dm,

I think that your code is not match with rotation Vector (in theory)
tmpZ = EstV.V.Z
EstV.V.Z = scos(deltaGyroAngle.A[ROLL]) * EstV.V.Z - ssin(deltaGyroAngle.A[ROLL]) * EstV.V.X; (eq 1)
EstV.V.X = ssin(deltaGyroAngle.A[ROLL]) * EstV.V.Z + scos(deltaGyroAngle.A[ROLL]) * EstV.V.X; (eq 2)

In eq2 the EstV.V.Z should be tmpZ . Why do you use EstV.V.Z ?
Regards,
Thanh Nguyen