This post is translated from the IMU (accelerometer and Gyroscope devices) guide used in embedded applications.
This article mainly introduces the mathematical models and basic algorithms of accelerometers and gyroscopes, and how to fuse the two, focusing on the discussion of algorithms and ideas.
Introduced
The purpose of this guide is to introduce inertial MEMS sensors, especially accelerometers and gyroscopes, and other integrated IMU (inertial measurement unit) devices to interested participants.
IMU Unit Example: the ACC Gyro 6DOF at the top of the MCU, named Usbthumb, supports usb/serial communication
In this article I will summarize a few basic and important topics:
-What the accelerometer (accelerometer) detects
-Gyroscope (gyroscope, also known as Gyro) detects what
-How to convert the data read from the sensor ADC to a physical unit (the unit of the accelerometer is G, the unit of the gyroscope is degrees per second)
-How to combine the accelerometer and gyroscope data to get accurate information about the inclination between the device and the ground plane
In the whole article I try to minimize the math operations. If you know what the sine, cosine, and tangent functions are, you should understand and use the ideas in this article regardless of which platform your project uses, such as Arduino, Propeller, Basic Stamp, ATEML chip, pic chip, and so on. Some people think that the use of IMU units requires complex mathematical operations (complex fir or IIR filtering, such as Kalman filtering, Parks-mcclellan filtering, etc.). You will get great and complicated results if you study these. The way I explain things, I just need basic math. I strongly believe in simple principles. I think a simple system is easier to operate and monitor, and many embedded devices do not have the ability or resources to implement complex algorithms that require matrix operations.
I will use a new IMU module I designed--acc_gyro Accelerometer + Gyro Imu as an example. In the example below we will use the parameters of this device. Using this module as an introduction is very suitable because it consists of 3 devices:
-Lis331al (datasheet) –3 axis 2G analog Accelerometer
-Lpr550al (datasheet) – Dual axis (tilt, roll) 500°/s acceleration sensor
-Ly550alh (datasheet) – Single axis (yaw) gyroscope the last device is not used in this introduction, but he plays an important role in DCM Matrix implementation.
Together they form an inertial measurement unit of 6 degrees of freedom. It's a fancy name! However, behind the fancy name is a very useful device combination, which we'll cover in detail next.
The first part of the accelerometer
To understand this module, we start with the accelerometer first. When we imagine an accelerometer we can think of it as a sphere in a square box. You might want to think of it as a cookie or a doughnut, but I'll just take it as a ball. Good:
We assume that the box is not in the gravitational field or anywhere else that affects the ball's position, and the ball is in the center of the box. You can imagine the box in outer space, away from any celestial body, if it is difficult to imagine, it is as a box in the space shuttle, everything is in a weightless state. In the above diagram you can see that we have allocated a pair of walls to each axis (we removed y+ to see what's going on inside). Imagine that you can sense pressure on every wall. If we suddenly move the box to the left (the acceleration is 1g=9.8m/s^2), then the ball will hit the X-wall. Then we detect the pressure on the wall, and the x-axis output value is -1g.
Note that the accelerometer detects that the right direction is the opposite of its own acceleration. This force is often referred to as inertia force or imaginary force. In this model you should learn that accelerometers measure acceleration by indirectly measuring the effect of force on a wall surface, and in practical applications, the force may be measured by means of a spring. This force can be caused by acceleration, but in the following example, we will find that it is not necessarily caused by acceleration.
If we put the model on Earth, the ball will fall on the z-wall and exert a 1g force on it, see:
In this case the box does not move but we still read the value of -1g to the z axis. The pressure exerted by the ball on the wall is caused by gravity. In theory, it can be different types of power-for example, you can imagine that our ball is iron, and a magnet is placed next to the box and the ball hits the other side of the wall. The reference to this example is just to show that the essence of the accelerometer is the detection force rather than the acceleration. Only the inertial force caused by acceleration can be captured by the accelerometer's detection device.
Although this model is not a real construction of a MEMS sensor, it is quite effective in solving problems related to accelerometers. In fact, some similar sensors have metal balls, which are called dip switches, but they are less functional, can only detect whether the device is tilted to a certain extent, but can not be tilted to a degree.
So far, we have analyzed the single-axis accelerometer output, which can be obtained using a single-axis accelerometer. The real value of a triaxial accelerometer is that they can detect the inertial forces of all three axes. Let's go back to the box model and rotate the box 45 degrees to the right. Now the ball will be in contact with two Faces: Z and x, see:
0.71G This value is not arbitrary, they are actually the approximate value of the square root of 1/2. This will be clearer when we introduce the next model of the accelerometer.
In the previous model we introduced gravity and rotated the box. In the last two examples we analyze the output value of the box in two cases, and the force vector remains the same. While this helps to understand how the accelerometer interacts with external forces, it is easier to calculate if we change the coordinate system to three axes of acceleration and imagine that the vector possibilities is rotated around.
Please look at the model above, I have retained the color of the axes so that your mind can better move from the previous model to the new model. Imagine that each axis in the new model is perpendicular to the respective wall of the original model. Vector r is the vector detected by the accelerometer (it may be gravity or the synthesis of inertial forces in the above example). Rx,ry,rz is the projection of vector r in X, Y, Z. Please note the following relationships:
, R ^ 2 = RX ^ 2 + RY ^ 2 + RZ ^ 2 (Equation 1)
This formula is equivalent to the three-dimensional space Pythagorean theorem.
Remember the 1/2 square root 0.71 I just said is not a random value. If you put them back up and look back on the gravitational acceleration of 1g, then we can verify that:
1 ^ 2 = (SQRT) ^ 2 + 0 ^ 2 + (SQRT (1/2)) ^ 2
InSimple substitution in Equation 1: r=1, Rx =-sqrt, Ry = 0, Rz =-sqrt (1/2)
After a long period of theoretical preface, we are very close to the actual accelerometer. The RX,RY,RZ value is the linear correlation value of the actual accelerometer output, which you can use for various calculations.
Before we use it, let's talk a little bit about how to get the accelerometer data. Most accelerometers can be categorized into two categories: digital and analog. The digital accelerometer can obtain information by I2C,SPI or Usart, and the output of the analog accelerometers is a voltage value within a predetermined range, and you need to convert it to a numeric value using the ADC (Analog to digital) module. I won't go into the details of how ADC works, in part because it's a very broad topic, and another reason is that there are differences between ADCs on different platforms. Some MCUs have built-in ADC modules, while others require an external circuit for ADC conversion. Regardless of the type of ADC module you are using, you will get a value within a certain range. For example, a 10-bit ADC module has a range of output values of 0. 1023 rooms, please note that 1023 = 2 ^ 10-1. The output value of a 12-bit ADC module ranges from 0. Within 4095, note that 4095 = 2 ^ 12-1.
Let's go ahead and consider the next simple example, assuming we've got the following three axes of data from the 10-bit ADC module:
ADCRX = 586
Adcry = 630
ADCRZ = 561
Each ADC module has a reference voltage, assuming that in our case it is 3.3V. To turn a 10-bit ADC value into a voltage value, we use the following formula:
VOLTSRX = ADCRX * vref/1023
Small bet: The maximum value for a 8-bit ADC is 255 = 2 ^ 8-1, and the 12-bit ADC maximum is 4095 = 2 ^ 12-1.
By substituting the values of the 3 axes, you get:
VOLTSRX = 586 * 3.3/1023 =~1.89v (results take two decimal places)
Voltsry = 630 * 3.3/1023 =~2.03v
VOLTSRZ = 561 * 3.3/1023 =~1.81v
Each accelerometer has a voltage value of 0 acceleration, which you can find in its manual, which corresponds to an acceleration of 0g. By calculating the offset of the relative 0g voltage we can get a signed voltage value. For example, the 0g voltage value vzerog= 1.65V, the following way can be obtained relative 0g voltage offset:
DELTAVOLTSRX = 1.89v-1.65v = 0.24V
Deltavoltsry = 2.03v-1.65v = 0.38V
Deltavoltsrz = 1.81v-1.65v = 0.16V
Now we have the accelerometer voltage, but its unit is not G (9.8m/s^2), the final conversion, we also need to introduce the accelerometer sensitivity (sensitivity), the unit is usually mv/g. For example, the accelerometer's sensitivity sensitivity= 478.5mv/g = 0.4785v/g. The sensitivity value can be found in the accelerometer specification. To get the last acceleration of the unit G, we use the following formula to calculate:
RX = deltavoltsrx/sensitivity
RX = 0.24v/0.4785v/g =~0.5g
RY = 0.38v/0.4785v/g =~0.79g
RZ = 0.16v/0.4785v/g =~0.33g
Of course, we can put all the steps in one formula, but I want to introduce each step to let you know how to read an ADC value and convert it to the component of the vector possibilities in G.
Rx = (ADCRX * vref/1023–vzerog)/Sensitivity(Equation 2)
Ry = (Adcry * vref/1023–vzerog)/Sensitivity
Rz = (ADCRZ * vref/1023–vzerog)/Sensitivity
Now we have three components of the inertial force vector, and if the device is not affected by any external forces except gravity, then we can assume that this direction is the direction of the gravitational vector. If you want to calculate the inclination of the device relative to the ground, you can calculate the angle between the vector and the z-axis. If you are interested in the inclination of each axis, you can divide the result into two components: x-axis, y-axis inclination, which can be obtained by calculating the angle between the gravitational vector and the x and Y axes. Calculating these angles is easier than you think, and now that we've calculated the RX,RY,RZ values, let's go back to our previous acceleration model and add some more annotations:
The angle we are interested in is the angle between the vector R and the x, Y, Z axis, which is AXR,AYR,AZR. Observe the right triangle consisting of R and Rx:
cos (AXR) = rx/r, similar to:
cos (Ayr) = ry/r
cos (AZR) = rz/r
From Equation 1 We can push export R = SQRT (rx^2 + ry^2 + rz^2)
Through the Arccos () function (the inverse function of the cos ()) we can calculate the desired angle:
AXR = Arccos (RX/R)
Ayr = Arccos (RY/R)
AZR = Arccos (RZ/R)
We have spent a lot of time explaining the accelerometer model, and all we need is the above formula. Depending on your application, you may be able to use some of the transition formulas that we have deduced. We will now introduce the Gyroscope module and introduce how to fuse the accelerometer and gyroscope data to get a more accurate dip value.
But before we do, we'll cover a couple of very common formulas:
cosx = cos (AXR) = rx/r
CosY = cos (Ayr) = ry/r
Cosz = cos (AZR) = rz/r
These three formulas are often referred to as the direction cosine, which mainly expresses the same orientation of the unit vectors (vectors with a length of 1) and the R vectors. You can easily verify:
SQRT (cosx ^ 2 + COSY ^ 2 + Cosz ^ 2) = 1
This is a good property because it avoids the modulus (length) of the R vector that we have been testing. Usually if we are only interested in the direction of the inertia force, it is a wise choice to standardize the die length to simplify other calculations.
Part II Gyroscope
For the gyroscope we will not introduce its equivalent box model like an accelerometer, but jump directly to the second model of the accelerometer, which we will introduce to you how the gyroscope works.
Each gyroscope's channel detects the rotation of an axis. For example, a 2-axis gyroscope detects rotation around the x and y axes. In order to use numbers to express these rotations, we first introduce some symbols. First we define:
Projection of rxz– inertial force vector R on the XZ plane
ryz– inertial force vector r projection on YZ plane
In the right triangle composed of rxz and RZ, the use of the Pythagorean theorem can be used:
rxz^2 = rx^2 + rz^2, same:
ryz^2 = ry^2 + rz^2
Also note:
r^2 = rxz^2 + ry^2, this formula can beFormula 1 and the above formula deduced, can also be deduced from the right triangle composed of R and Ryz
R ^ 2 = Ryz ^ 2 + RX ^ 2
We don't use these formulas in this article, but we know that the relationships between those values in the model are helpful to understand.
Instead, we define the angle between the z-axis and the RXZ, Ryz vectors as follows:
AXZ-RXZ (the projection of vector R in the XZ plane) and the angle of the z axis
Ayz-ryz (Vector r in the YZ plane projection) and z-axis angle
Now we're a step closer to what the gyroscope is measuring. The gyroscope measures the rate of change of the angle defined above. In other words, it outputs a value that is linearly related to the rate of change in the above angles. To explain this, we first assume that at the t0 moment, we have measured the angle of rotation around the y axis (i.e. AXZ), defined as Axz0, and then we measure this angle again at T1 time and get Axz1. The rate of change in angle is calculated as follows:
Rateaxz = (axz1–axz0)/(T1–T0).
If the degree is used to represent the angle, the seconds to represent the time, then the unit of the value is degrees per second. This is what the gyroscope detects.
In practice, the gyroscope will not give you a unit-per-second value directly (unless it is a special digital gyroscope). Just like an accelerometer, you get an ADC value and you want to use a similarEquation 2 converts it to a value in degrees per second. Let's introduce the ADC portion of the Gyroscope output value conversion (assuming a 10-bit ADC module, 8-bit ADC, 1023 instead of 255 if 12 is the ADC with 4095 instead of 1023).
RATEAXZ = (ADCGYROXZ * vref/1023–vzerorate)/SensitivityEquation 3
Rateayz = (Adcgyroyz * vref/1023–vzerorate)/Sensitivity
Adcgyroxz,adcgyroyz-These two values are read by the ADC, which represent the angle of the vector r projection in the XZ and YZ planes, or, equivalently, the rotation can be decomposed into a single movement around the y and x axes.
VREF–ADC reference voltage, in the above example we use 3.3V
vzerorate– is a 0 change-rate voltage, in other words it is the output value of the gyroscope is not affected by any rotation, for the ACC Gyro Board, it can be considered 1.23V (this value can usually be found in the manual-but do not believe this value, because most of the gyroscope after welding will have a certain deviation, So you can use a voltmeter to measure the output value of each channel, usually this value will not change after soldering, if there is a bounce, before the device is used to write a calibration program to measure it, the user should keep the device stationary for calibration when the device is started.
The sensitivity of the sensitivity– gyroscope, Unit mv/(DEG/S), is usually written mv/deg/s, which means that if the speed of rotation increases 1°/s, the output of the gyroscope increases by how many mv. The sensitivity value of the Acc_gyro plate is 2mv/deg/s or 0.002v/deg/s
Let us give an example, assuming that our ADC module returns the following values:
ADCGYROXZ = 571
ADCGYROXZ = 323
With the above formula, the parameters of the ACC Gyro Board can be used in substituting:
RATEAXZ = (571 * 3.3v/1023–1.23v)/(0.002v/deg/s) =~ 306 deg/s
Rateayz = (323 * 3.3v/1023–1.23v)/(0.002v/deg/s) =~ -94 deg/s
In other words, the device rotates around the y-axis (or within the XZ plane) at 306°/s speed and around the x-axis (or in the YZ plane) at -94°/s speed. Note that the minus sign indicates that the device is rotated in the opposite direction. By convention, rotation in one direction is positive. A good gyroscope specification will tell you which direction is positive, otherwise you'll have to test which direction of rotation will cause the output foot voltage to increase. It is best to test with an oscilloscope, because once you stop the rotation, the voltage will fall back to the 0 rate level. If you are using a multimeter, you have to maintain a certain rotational speed for a few seconds and compare the voltage value and the 0-rate voltage value at the same time. If the value is greater than 0 rate the voltage value that indicates that the direction of rotation is positive.
the third part is to synthesize them together. The data of the accelerometer and gyroscope are fused.
If you are reading this article you may already have or are ready to purchase an IMU device, or you are ready to build one with a separate accelerometer and gyroscope.
Note: For specific code implementation and algorithmic testing, please read this article:
Http://starlino.com/imu_kalman_arduino.html
When using IMU devices that incorporate accelerometers and gyroscopes, the first thing to do is to unify their coordinate systems. The simplest way is to use the accelerometer as a reference coordinate system. Most accelerometer specifications will indicate the xzy axis orientation corresponding to the physical chip or device. For example, the following is the XYZ axis direction given in the specification of the ACC Gyro Board:
The next steps are:
-Determine the output of the gyroscope corresponds to the Rateaxz,rateayz value discussed above.
-Determine if the output value should be reversed depending on the position of the gyroscope and accelerometer
Do not assume that the output of the gyroscope is XY, which adapts to any axis in the accelerometer coordinate system, although this output is part of the IMU module. The best way is to test.
The next example is used to determine which gyroscope's output corresponds to the RATEAXZ.
-Keep the device level first. The XY axis output of the accelerometer will be 0 acceleration voltage (ACC Gyro Board value is 1.65V)
-Next rotate the device around the y-axis, in other words, rotate the device in the XZ plane, so the X and z acceleration output values change and the y-axis remains the same.
-When rotating the device at a constant speed, note which channel output value of the gyroscope has changed, and the other outputs should remain unchanged.
-When the gyroscope rotates around the y-axis (rotated within the XZ plane), the output value changes to ADCGYROXZ, which is used to calculate the RATEAXZ
-The final step is to determine if the direction of rotation corresponds to our model, because the position of the gyroscope and acceleration, sometimes you may want to reverse the RATEAXZ value
-Repeat the above test to rotate the device around the y-axis, this time looking at the x-axis output of the accelerometer (i.e. ADCRX). If the ADCRX increases (the first 90° starting at the horizontal position), the ADCGYROXZ should be reduced. This is because we are looking at the gravitational vector, and the vector rotates in the opposite direction when the device rotates in one direction (relative coordinate system motion). So, if you don't want to reverse rateaxz, you canEquation 3To solve this problem by introducing the sign in:
RATEAXZ = Invertaxz * (ADCGYROXZ * vref/1023–vzerorate)/sensitivity, where invertaxz= 1 or 1
The same method can be used to test the Rateayz and rotate the device around the x-axis, and you can measure which output of the gyroscope corresponds to the rateayz and whether it needs to be reversed. Once you have identified the Invertayz, you can use the following formula to calculate the Rateayz:
Rateayz = Invertayz * (Adcgyroyz * vref/1023–vzerorate)/Sensitivity
If you perform these tests on the ACC Gyro Board, you will get the following results:
-The output pin of the rateaxz is GX4,INVERTAXZ = 1
-Rateayz Output pin is Gy4,invertayz = 1
From now on we think you've set up the IMU module and can calculate the correct AXR,AYR,AZR value (as defined in the first part of the accelerometer) and Rateayz,rateayz (in the second part of the gyroscope). Next, we analyze the relationships between these values and get a more accurate inclination between the device and the ground plane.
You may ask yourself a question, if the accelerometer has already told us the inclination of the AXR,AYR,AZR, why bother to get the gyroscope data? The answer is simple: The accelerometer's data is not 100% accurate. For several reasons, it is also remembered that the accelerometer measures the inertial force, which can be caused by gravity (ideally only by gravity), when it may also be caused by acceleration (motion) of the device. Therefore, even if the accelerometer is in a relatively stable state, it is sensitive to general vibration and mechanical noise. This is why most IMU systems require a gyroscope to make the output of the accelerometer smoother. But how to do this? Is the gyroscope unaffected by noise?
The gyroscope also has noise, but because it detects rotation, it is less sensitive to linear mechanical motion, but there is another problem with the gyroscope, such as drift (the voltage does not go back to the 0 rate voltage when the selection stops). However, by calculating the average of the accelerometer and gyroscope we can get a relatively accurate inclination value for the current device, which is better than using an accelerometer alone.
Next step I will introduce an algorithm that is inspired by some of the ideas in Kalman filtering, but it is simpler and easier to implement in embedded devices. Before that, let's take a look at what we need to calculate the value of the algorithm. The calculation is gravity vector r=[rx,ry,rz], it can be deduced from other values, such as AXR,AYR,AZR or Cosx,cosy,cosz, by these values we can get the device relative to the inclination of the plane, these relationships we have discussed in the first part. One might say--according to the first part of theEquation 2 Did we get the value of Rx,ry,rz? Yes, but remember, these values are only derived from the accelerometer data, and if you use them directly in your program you will get unbearable noise. To avoid further confusion, we redefine the measured values of the accelerometer:
The racc– is a vector of inertial forces measured by an accelerometer, which can be decomposed into the following components (projections on the XYZ axis):
RXACC = (ADCRX * vref/1023–vzerog)/Sensitivity
RYACC = (Adcry * vref/1023–vzerog)/Sensitivity
RZACC = (ADCRZ * vref/1023–vzerog)/Sensitivity
Now we have a set of values that come from only the accelerometer ADC. We call this set of data "vector" and use the following symbol:
RACC = [RXACC,RYACC,RZACC]
Because the components of these RACC can be obtained by the accelerometer data, we can think of it as an input to the algorithm.
Note that RACC measures gravity, and if you get the length of the vector approximately equal to 1g then you are right:
| racc| = SQRT (rxacc^2 +ryacc^2 + rzacc^2),
However, it is important to make sure that the vector is converted to the following vector:
RACC (normalized) = [rxacc/| racc| , ryacc/| racc| , rzacc/| Racc|].
This ensures that the standardized RACC is always 1.
And then we introduce a new vector:
Rest = [Rxest,ryest,rzest]
This is the output value of the algorithm, which is corrected by the gyroscope data and based on the last estimated value.
This is what the algorithm does:
-The accelerometer tells us: "Your position is RACC."
We said, "Thank you, but let me confirm."
-then correct this value based on the gyroscope's data and the last rest value and output the new estimate rest.
-We consider rest to be the "best value" for the current device posture.
Let's see how it's implemented.
At the beginning of the sequence, we first think that the acceleration value is correct and is assigned:
Rest (0) = RACC (0)
Rest and RACC are vectors, so the above equation can be replaced with 3 simple formulas, so be careful not to repeat them:
Rxest (0) = RXACC (0)
Ryest (0) = RYACC (0)
Rzest (0) = RZACC (0)
Next we make a measurement at each equal time interval T-second, get the new measured value, and define it as RACC (1), RACC (2), RACC (3) and so on. At the same time, at each interval we also calculate a new estimate of rest (1), rest (2), rest (3), and so on.
Suppose we were in nth step. We have two columns of known values that can be used:
Rest (n-1) – Previous estimate, rest (0) = RACC (0)
RACC (n) – Current accelerometer measurements
Before we calculate rest (n), we introduce a new value, which can be valued by the gyroscope and the previous estimate.
Called Rgyro, it is also a vector and consists of 3 components:
Rgyro = [Rxgyro,rygyro,rzgyro]
We calculate the component of this vector separately, starting with Rxgyro.
First look at the following relationship in the gyroscope model, according to the right triangle of RZ and RXZ, we can launch:
Tan (AXZ) = Rx/rz = Axz = atan2 (RX,RZ)
You may never have used the ATAN2 function, which is similar to Atan, but the Atan return value range is (-PI/2,PI/2), the atan2 return value range is (-PI,PI), and he has two parameters. It converts the Rx,rz value to the angle in 360° (-PI,PI). For more information, please read atan2.
So, knowing Rxest (n-1) and Rzest (n-1) we found:
AXZ (n-1) = atan2 (Rxest (n-1), Rzest (n-1)).
Remember, the gyroscope measures the axz angle change rate, so we can estimate the new angle AXZ (n) as follows:
AXZ (n) = Axz (n-1) + RATEAXZ (n) * T
Keep in mind that the RATEAXZ can be read by the Gyroscope ADC. A more accurate formula can be obtained by using the average speed:
Rateaxzavg = (Rateaxz (N) + RATEAXZ (N-1))/2
AXZ (n) = Axz (n-1) + Rateaxzavg * T
In the same vein, you get:
Ayz (n) = Ayz (n-1) + Rateayz (n) * T
OK, now we have AXZ (n), Ayz (n). Now how do we derive the Rxgyro/rygyro? According toEquation 1 We can write the Rgyro length in the following style:
| Rgyro | = SQRT (rxgyro ^ 2 + Rygyro ^ 2 + Rzgyro ^ 2)
At the same time, since we have standardized RACC, we can assume that it is 1 in length and will remain unchanged after rotation, so it is relatively safe to write in the following way:
| Rgyro | = 1
For the time being, we use shorter symbols for the following calculations:
X =rxgyro, Y=rygyro, Z=rzgyro
Depending on the relationship above, you can:
x = X/1 = X/sqrt (x^2+y^2+z^2)
Numerator denominator divided by sqrt (X ^ 2 + Z ^ 2)
x = (x/sqrt (x^2 + z^2))/SQRT ((x^2 + y^2 + z^2)/(x^2 + z^2))
Note X/sqrt (x^2 + z^2) = sin (axz), so:
x = sin (axz)/SQRT (1 + y^2/(x^2 + z^2))
Multiply the numerator denominator of the inner part of the sqrt by the z^2
x = sin (axz)/SQRT (1 + y^2 * z ^2/(z^2 * (x^2 + z^2)))
Note that z/sqrt (x^2 + z^2) = cos (axz), y/z = Tan (Ayz), so you can end up with:
x = sin (axz)/SQRT (1 + cos (AXZ) ^2 * TAN (ayz) ^2)
Replace with the original symbol can be:
Rxgyro = sin (axz (n))/SQRT (1 + cos (AXZ (n)) ^2 * TAN (Ayz (n)) ^2)
In the same vein, you get:
Rygyro = sin (Ayz (n))/SQRT (1 + cos (ayz (n)) ^2 * TAN (AXZ (n)) ^2)
Tip: This formula can be further simplified. Divide both sides with sin (axz (you)) to get:
Rxgyro = 1/sqrt (1/sin (AXZ (n)) ^2 + cos (AXZ (n)) ^2/sin (AXZ (n)) ^2 * TAN (Ayz (n)) ^2)
Rxgyro = 1/sqrt (1/sin (AXZ (n)) ^2 + cot (AXZ (n)) ^2 * sin (Ayz (n)) ^2/cos (Ayz (n)) ^2)
Now add minus cos (AXZ (n)) ^2/sin (AXZ (n)) ^2 = Cot (AXZ (n)) ^2
Rxgyro = 1/sqrt (1/sin (AXZ (n)) ^2-cos (AXZ (n)) ^2/sin (AXZ (n)) ^2 + cot (AXZ (n)) ^2 * sin (Ayz (n)) ^2/cos (Ayz (n)) ^2 + Cot (AXZ (n)) ^2)
General Conditions 1, 2 and 3, 4 are available:
Rxgyro = 1/sqrt (1 + cot (AXZ (n)) ^2 * SEC (Ayz (n)) ^2), where cot (x) = 1/tan (x), sec (x) = 1/cos (x)
This formula uses only 2 trigonometric functions and has a lower computational capacity. If you have a Mathematica program, you can verify this formula by using Fullsimplify [sin[a]^2/(1 + cos[a]^2 * tan[b]^2)].
Now we find:
Rzgyro = sign (Rzgyro) *sqrt (1–rxgyro^2–rygyro^2).
Where, when Rzgyro>=0, sign (Rzgyro) = 1, when rzgyro<0, sign (rzgyro) =-1.
A simple method of estimating:
Sign (Rzgyro) = sign (Rzest (n-1))
In practical applications, beware of Rzest (n-1) approaching 0. At this point you can skip the entire gyroscope phase and assign a value: Rgyro=rest (n-1). RZ can be used as a reference for calculating axz and ayz inclination, and when it approaches 0 o'clock, it may overflow and cause bad consequences. At this point you get very large floating point data, and the result of the tan ()/atan () function lacks precision.
Now we look back at the results we've got, we're in the nth step of the algorithm, and we've calculated the following values:
racc– the current value of the accelerometer read
rgyro– based on rest (-1) and current gyroscope read values
Which value do we update with rest (n)? As you might have guessed, both are used. We will use a weighted average value to:
Rest (n) = (RACC * W1 + rgyro * W2)/(W1 + w2)
The numerator denominator is divided by W1, and the formula can be simplified into:
Rest (n) = (RACC * w1/w1 + rgyro * w2/w1)/(W1/W1 + w2/w1)
Make W2=w1=wgyro, can get:
Rest (n) = (RACC + rgyro * wgyro)/(1 + wgyro)
In the above formula, Wgyro shows how much we believe in accelerometers and gyroscopes. This value can be determined by testing, and a good result is obtained based on the experience value of 5-20.
The main difference between this algorithm and the Kalman filter is that its weights are relatively fixed, while the weights in the Kalman filter change with the noise that the accelerometer reads. The Kalman filter focuses on giving you a "best" theoretical result, and this algorithm gives you the "enough" result of the actual project. You can implement an algorithm that can change the Wgyro value based on the measured noise, but the fixed weights for most applications can work well.
Getting the latest estimate now is one step further:
Rxest (n) = (RXACC + rxgyro * wgyro)/(1 + wgyro)
Ryest (n) = (RYACC + rygyro * wgyro)/(1 + wgyro)
Rzest (n) = (RZACC + rzgyro * wgyro)/(1 + wgyro)
Now, normalize the vectors again:
R = SQRT (Rxest (n) ^2 + ryest (n) ^2 + rzest (n) ^2)
Rxest (n) = Rxest (n)/R
Ryest (n) = Ryest (n)/R
Rzest (n) = Rzest (n)/R
Now it's time to do the next round of loops again.
Note:For specific implementations and tests of this algorithm, please read this article:
Http://starlino.com/imu_kalman_arduino.html
Additional resources for accelerometer and gyroscope IMU Fusion:
Http://www.mikroquad.com/pub/Res ... ryfilter/filter.pdf
Http://stackoverflow.com/questio ...-accelerometer-data
Http://www.dimensionengineering.com/accelerometers.htm
Gyro Principle--Online reprint
Start building with 50+ products and up to 12 months usage for Elastic Compute Service
1 on 1 presale consultation
24/7 Technical Support 6 Free Tickets per Quarter Faster Response
Alibaba Cloud offers highly flexible support services tailored to meet your exact needs.
A comprehensive suite of global cloud computing services to power your business
Payment Methods We Support
