After reading this, let you get the compass and meter calibration in minutes!
The calibration of compass and accelerometer is the most basic work in daily development. Teacher Echo is specially invited to summarize the engineering method of compass and accelerometer calibration. This article is for you to solve your doubts.
author information
Echo, whose real name is Zou Jiachi, is engaged in embedded software development.
Contact: QQ 5 29380360
Super detailed explanation: compass and accelerometer calibration method
(Attach C source code)
1. Why calibration
We all know that the compass measures the strength of the surrounding magnetic field. If there is no interference from external magnetic fields and only geomagnetism exists, theoretically the magnetic field measured by the rotation of the compass is a sphere.
However, in real space, in addition to the geomagnetic field, there are other magnetic interferences. Here I simply divide it into two categories.
The first category : the magnetic field in the earth's space. This kind of magnetic field has a characteristic, that is, with the rotation of the compass coordinate system, the direction of the magnetic field does not change, similar to the geomagnetic field.
The second category : the magnetic field in the compass coordinate space. This kind of magnetic field source is generally fixed on the airplane, so as the compass coordinate system rotates, the direction of the magnetic field also rotates. But for the compass coordinate system, it is a constant value.
For the first type of magnetic field, there is no good way to correct it (if any god knows, please let me know). The correction method I will introduce later is to filter out the interference of the second type of magnetic field. The idea of correction is the ellipsoid fitting algorithm based on the least squares method.
Note: This can only correct the magnetic field with a fixed magnetic field strength. I have not tested the changing magnetic field of the motor. I don’t know how the strength of the magnetic field generated by the motor is related to the motor speed. If anyone has studied it, Please also inform, thank you.
2. Derivation of ellipsoid fitting correction theory
There are many papers on ellipsoid fitting and correction on the Internet, but I didn’t read them carefully, because those formulas are very obscure and difficult to understand. Without that patience, I try to use the simplest language to introduce the theoretical basis of the correction method.
First establish a numerical model and set the measured value as:
,The corrected value is:,
The translation parameters are:
The scaling parameters are:
The relationship between them is as follows:
Our goal after correction is to make the correction value approximately distributed on a sphere, and everyone knows the formula of the sphere: x2+y2+z2=R2, so we bring the corrected value into the sphere formula, which is consistent with the theoretical The square of the radius of the sphere is the difference to construct the error u:
Replace the corrected value with the measured value to become:
As you can see, this is clearly an ellipse formula~
Remember:
Then our error u can be written as follows:
The following is the core idea of the correction: Suppose we have many sets of data, and we require a set of parameters to minimize the error sum of all data, that is, ∑u is the smallest, but because u has positive and negative signs, errors with opposite signs are possible Cancel each other out. What about adding absolute value? This is also undesirable, because it is very complicated to minimize the absolute value function. Then we naturally think of finding the sum of squares of u, namely:
U = ∑u ^ 2
We regard u as an unknown number. This function is a quadratic function with a minimum point. In order to find this minimum point, we can do a partial derivative of it:
Remember:
Then we can write the partial derivative as follows:
B is known, and P is the parameter matrix we want to find, so the solution of each parameter of P can be obtained by seeking a homogeneous linear equation system.
I will not explain the process of solving homogeneous linear equations in detail here. The method used in my algorithm is the classical Gaussian elimination method. If you are interested, you can take a closer look.
When we find the unknowns a, b, c, d, e, f, g of P, we need to use these parameters to find our offset (ox, oy, oz) and zoom (gx, gy,gz).
Before the inverse solution, we first return to the previous formula, BxP=0. In fact, the solution P that satisfies this formula has an array, we can rewrite the formula as BxCP=0, and C is an arbitrary constant, namely
What we find by solving linear equations is only a basic solution in this solution system, so we first ask for C of this basic solution.
The formula is deduced as follows, and it can be solved by entering a, b, c, d, e, f, g:
C=(d2/a + e2/b + f2/c-4g)/4R2 (R is the theoretical sphere radius)
ox=d/2a
oy=e/2b
oz=f/2c
gx=sqrt(a/C)
gy=sqrt(b/C)
gz=sqrt(c/C)
Finally, callback these six parameters to the previous formula to complete the correction.
3. Correction program source code (C language)
If the code is too long, I won’t post it. You can download it on the web disk.
Compass and accumulator calibration method C source code
http://pan.baidu.com/s/1dFiB6z3
According to the principle of this algorithm, a calibration program written for a high-performance gyroscope:
Gyro:
Algorithm display:
