SparkFun Forums

[sfe_kav]

Hello,
I am porting algorithm from http://code.google.com/p/sf9domahrs/ to my device

I have angular velocity measured in rad/s, acceleration in G units (1==9.8m/sec^2), and magnetic field in units of earth mag. field

Is anybody know physical and/or mathematical meaning of these four coefficient:

#define Kp_ROLLPITCH 0.02
#define Ki_ROLLPITCH 0.00002
#define Kp_YAW 1.2
#define Ki_YAW 0.00002

Thanx.

[Cilph]

Looks like some PID constants for proportional and integral control.

[sfe_kav]

yes this is proportional and integral constants for drift coorection, but what they mean?))) (elefant)/(monkey square)?))

[sebmadgwick]

Kp is a proportional gain and Ki is an integral gain. It appears that in this algorithm the developer has decoupled yaw from pitch and roll. I suspect that this may not be an ideal solution. See my code for similar alternatives that maintain coupling throughout:
http://code.google.com/p/imumargalgorithm30042010sohm/
and
http://www.x-io.co.uk/res/doc/quaternions.pdf

To answer your question:

Kp governs the rate at which the estimated orientation (of the sensor array) converges to the orientation assumed by measurements of the field alone (either gravity or magnetic). A Kp value of zero will mean that only gyroscope values are used - you may as well disconnect the field sensors (accelerometer/magnetometer). A large value will mean that the gyroscope is 'trusted less' but your estimate of orientation will be corrupted by erroneous field measurements; i.e. linear accelerations due to motion or local magnetic distortions.

Ki is an integral gain. This gain governs the rate at which a corrective feedback term increases when there are non-mean-zero errors in the estimated orientation. These corrective terms will therefore remove bias errors in the estimated rate of change of orientation; i.e. compensate for gyroscope bias drift.

Note that with only Kp (i.e. Ki = 0), the system is a first order control system and is therefore intrinsically stable. If Ki is non-zero then the system is second order and may become unstable. You can get a feel for this by trying different gains. If Ki is relatively close to Kp (e.g. Kp = 5, Ki = 2) then you will observe that the estimated orientation will oscillate before reaching a steady state. If the gains are inappropriately chosen the system can become critically stable or unstable; that is, it can oscillate forever or oscillate with an increasing magnitudes tending to infinity.

[sfe_kav]

sebmadgwick, thanks.

Very helpfull for me.

[demss]

Hi,

I read the MARGdemo code.
http://code.google.com/p/imumargalgorit ... loads/list

in this file: Form1.cs

Code: Select all

// sensor calibration variables and constants
        private const double a_xBias = 32634.2779917682;            // accelerometer bias
        private const double a_yBias = 32300.1140276867;
        private const double a_zBias = 32893.0853282136;
        private const double a_xGain = -0.00150042985864975;        // accelerometer gains
        private const double a_yGain = -0.00147414192905898;
        private const double a_zGain = 0.00152294825926844;
        private double w_xBias = 25247;                             // gyroscope bias
        private double w_yBias = 25126;
        private double w_zBias = 24463;
        private const double w_xGain = 0.00102058528925813;         // gyroscope gains
        private const double w_yGain = -0.00110455853342484;
        private const double w_zGain = 0.00107794298635984;
        private const double m_xBias = -8.20750399495073;           // magnetometer baises
        private const double m_yBias = 15.6531909021474;
        private const double m_zBias = 7.32498941411782;
        private const double m_xGain = -0.00160372297752976;        // magnetometer gains
        private const double m_yGain = 0.0016037818986323;
        private const double m_zGain = 0.00182483736430979;

My questions is,
What is a Bias and a Gains for (accelerometer, Gyroscope, magnetometer)?
How do I calculate The Bias and the Gains if I have other kind of captors?

Thank you

[sfe_kav]

bias == zero value


A=(ADC_VAL-BIAS)*GAIN
where A - is acceleration(m\c^2) or angular velocity(deg/s) or gauses
ADC_VAL - value from analog to figital converter (for arduino in range 0-1023)
BIAS -zero value
GAIN proportial coefficient to convert adc value to phisical units(acceleration, ang. velocity or gauses)
for bias and gain values read datasheet for sensor device

in datasheet bias and gains in phisical units per volt
for example for accelerometer : 0.5V/G (where G== 9.8 m/c^2) and sero value 1.5V
in this situation for arduino adc:
bias= 1.5V/5V *1023
gain= 0.5V/5V *1023

where 5V - reference voltage for arduino.

[sfe_kav]

sorry.

bias=1.5V/5V*1023
gain=5V/(0.5V*1023)

[demss]

Thank you sfe_kav for your help!

If I use a 3.3V arduino, the analogic Accelerometer calcul is?
bias=1.5V/3.3V*1023
gain=3.3V/(0.5V*1023)

I find your sero value 1.5V in page 3 of this ADXL335 Datasheet.
Your 0.5V/G (I not find in Datasheet) it is just for divide by 2 the analogic resolution?


If I use a 3.3V arduino with a Digital I2C Accelerometer ADXL345, How do I calculate The Bias and the Gains with this captor?
I find the XY sero value = 40mg and Z sero value = 80mg in page 3 of this ADXL345 Datasheet.

Thank you

[sfe_kav]

for adxl335 gain is (from specification line about SENSITIVITY 300mV/G == 0.3V/G)
so gain will be 3.3V/(0.3V*1023)

For adxl345 you read digital values from sensor
sensitivity in specification pointed in LSB/G (lowerest significant bit/G)
so for +-2G measurement range and 10 bit resolution sensitivity is 256 LSB/G
zero(bias) value i think ==0 (signed value read from accelerometer)
bias=1/256

But read carefully about data format in registers DATAX0 DAAX1 and other axis!

[sfe_kav]

sorry))))

for adx345 read scale line from specification
for +-2G 10 bit resolution mode scale=3.9mG/LSB ==0.0039G/LSB
so gain=scale=0.0039 )))

here is tutorial for adxl345 http://www.sparkfun.com/tutorials/240

[sfe_kav]

)))) 1/256 == 0.0039
my english is vary bad))) its the same value but inverted)))


in first post about adxl345:
[remove]bias=1/256[/remove] gain=1/256
bias = 0

[demss]

Thank you sfe_kav for your help and for the Tutorial!

Me too my english is bad.
I speak french.