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[3];
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 means
void 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