imu calibration

13
Calibration of IMU sensors By Aniket Govindaraju Research Assistant, ECE Controls and Robotics Research Lab NYU Polytechnic School of Engineering Working under: Prof Farshad Khorrami Controls and Robotics Research Lab NYU Polytechnic School of Engineering

Upload: aniket-govindaraju

Post on 13-Apr-2017

53 views

Category:

Documents


0 download

TRANSCRIPT

Page 1: IMU Calibration

Calibration of IMU sensors

By Aniket Govindaraju Research Assistant, ECE

Controls and Robotics Research Lab NYU Polytechnic School of Engineering

Working under: Prof Farshad Khorrami Controls and Robotics Research Lab

NYU Polytechnic School of Engineering

Page 2: IMU Calibration

Abstract: IMU sensors such as magnetometers, accelerometers and gyroscopes are used for navigation and measuring the velocity, orientation and magnetic field of a device. But the measured values of these sensors are corrupted by various errors such as instrument noises, constant bias, scale factor deviations and non-orthogonality of the sensor axes. The calibration of the magnetometer and accelerometer can be done in two parts; correcting the constant bias and fitting the data which is ellipsoidal into a sphere of the proper radius (magnetic field strength for magnetometer and gravitational pull for accelerometer). After calibration, the orientation (roll, pitch, yaw) of the instrument can be calculated with the minimal error. To evaluate the performance if the calibration procedure, simulations were performed in MATLAB with data sets from the sensors. The results show that the calibration procedure is successful and stable.

Key words: Magnetometer, Accelerometer, Calibration, Orientation (roll, pitch, yaw)

Table of contents:

Abstract: ....................................................................................................................................... 2

Key words: ................................................................................................................................... 2

Table of contents: ..................................................................................................................... 2

Introduction: .............................................................................................................................. 3 Magnetometer: .................................................................................................................................... 3

Hard Iron: .......................................................................................................................................................... 4 Soft Iron: ............................................................................................................................................................ 4

Accelerometer: .................................................................................................................................... 8 Calibration: ....................................................................................................................................................... 8

Calculation of Roll, Pitch and Yaw angles: .............................................................................. 11 Development of GUI: ....................................................................................................................... 11

Conclusion: ............................................................................................................................... 12

References:............................................................................................................................... 13

Acknowledgements: ............................................................................................................. 13

Page 3: IMU Calibration

Introduction: A tilt-compensated electronic compass is implemented using the combination of a three-axis accelerometer and a three-axis magnetometer. The accelerometer readings provide pitch and roll angle information, which is used to correct the magnetometer data. This allows for accurate calculation of the yaw or compass heading when the compass is not held flat. The magnetometer readings must be corrected for Hard-Iron and Soft-Iron effects and the accelerometer readings corrected for constant bias and scale factor. A positive yaw angle is defined to be a clockwise rotation about the positive z-axis. Similarly, a positive pitch angle and positive roll angle are defined as clockwise rotations about the positive y- and positive x-axes respectively.

Magnetometer: Magnetometer readings, in absence of any errors can be defined by;

Bp = Rx(roll)*Ry(pitch)*Rz(yaw)*Br Eqn 1

Page 4: IMU Calibration

Where Bp is the Magnetometer reading, Rx, Ry, Rz are the rotation matrices about X, Y and Z axes with the angles roll, pitch and yaw respectively, Br is the local geomagnetic vector of magnitude B and inclination I at the sensor location.

Br = B*[cos(I); 0; sin(I)] Eqn 2

Magnetometer is affected by two main errors in its readings. They are called the Hard Iron and Soft iron offsets.

Hard Iron: The hard-iron offset is due to the permanently magnetized ferromagnetic components on the PCB. Since the magnetometer and PCB rotate together, the hard-iron offset is a simple vector V(1), which adds to the magnetometer reading. Also, any zero field offset in the magnetometer factory calibration will appear as a fixed additive vector V(2). For convenience, both offsets are normally combined as a single 'hard-iron' vector V defined as;

V = V(1) + V(2) Eqn 3

Including this Hard Iron offset in Eqn 1, we get

Bp = Rx(roll)*Ry(pitch)*Rz(yaw)*Br + V Eqn 4

Soft Iron: The soft-iron effect is due to the interfering magnetic field induced by the geomagnetic field onto normally unmagnetized ferromagnetic components on the PCB. The assumption is made that the induced soft-iron field is linearly related to the geomagnetic field measured in the rotated reference frame by the 3 by 3 matrix W. The linearity assumption is generally accurate but is not strictly true in the presence of magnetic hysteresis effects present in either the magnetometer or in ferromagnetic components on the PCB. Including the Soft Iron effect in Eqn 4, we get

Bp = W*Rx(roll)*Ry(pitch)*Rz(yaw)*Br + V Eqn 5

Calibration: As we know from Eqn 1, the correct readings of the magnetometer, Bpc is given by;

Page 5: IMU Calibration

Bpc = Rx(roll)*Ry(pitch)*Rz(yaw)*Br

Therefore by modifying Eqn 5, we see that

Bpc = inv(W)*(Bp-V) Eqn 6

Calculating the Hard Iron Matrix: Using Eqn 5,

Bp = W*Rx(roll)*Ry(pitch)*Rz(yaw)*Br + V

Inv(W)*(Bp-V) = Rx(roll)*Ry(pitch)*Rz(yaw)*Br

(Inv(W)*(Bp-V))’*(Inv(W)*(Bp-V)) = (Rx(roll)*Ry(pitch)*Rz(yaw)*Br)’* (Rx(roll)*Ry(pitch)*Rz(yaw)*Br)

(as we know, rotation matrices are orthogonal matrices, i.e, R’ = inv(R))

(Bp-V)’*inv(W)’*inv(W)*(Bp-V) = B^2

(let inv(W)’*inv(W) = A)

(Bp-V)’*A*(Bp-V) = constant

Eqn 7 Eqn 7 is the equation of an ellipsoid with center at V and radius equal to the constant on the RHS. Hence, by finding the center of the magnetometer data, Hard Iron Matrix can be obtained. As for any ellipsoid,

Center = V = (Max(X,Y,Z)+Min(X,Y,Z))/2 Eqn 8

From Eqn 8, Hard Iron Matrix (V) is calculated. Calculating the Soft Iron Matrix: Let Bc be the magnetometer data after Hard Iron Correction.

Bc = Bp-V

Page 6: IMU Calibration

If the constraint is applied that inv(W) is symmetric, then it is impossible for any spurious rotation angle and error in compass heading to be introduced by the calibration process since any rotation matrix must be anti-symmetric. A further understanding of the advantage of a symmetric inverse soft-iron matrix can be obtained by relating the eigenvectors of the matrix A to the eigenvectors of the inverse soft-iron matrix inv(W). The eigenvectors G and eigenvalues U of the inverse soft-iron matrix inv(W) are defined by:

Inv(W)*G = U*G Eqn 9

As we know from Eqn 7,

Bc’*A*Bc = B^2

(A = inv(W)’*inv(W)) Eqn 10

Using Kronecker Product and Vectorization property;

Vec(AXB) = (B’(*)A)*vec(X)

(where (*) indicates kronecker product)

Vec(Bc’*A*Bc) = Vec(B^2)

(Bc’(*)Bc’)*vec(A) = B^2

vec(A) = pinv(Bc’*Bc’)*B^2 Eqn 11

This will give us the vector A of size 9x1, which when reshaped into a 3x3 matrix forms the matrix A (Eqn 10). Given that inv(W) is symmetric then: i) The eigenvectors of the matrix A are identical to the eigenvectors G of inv(W) ii) The eigenvalues of A are the square of the eigenvalues U of inv(W) Hence, by finding the eigenvectors and eigenvalues of A, we can obtain the matrix inv(W). Let eigenvalues of A be O;

Inv(W) = G*O^0.5*inv(G) Eqn 12

Using the results from Eqn 8 and Eqn 12 in Eqn 6,

Page 7: IMU Calibration

Bpc = inv(W)*(Bp-V)

The calibrated magnetometer data is obtained. The above procedure was followed using MATLAB and sensor data from a 3-axis Magnetometer. The results were as such;

i. 2-D Magnetometer Calibration

Above: (Left) Raw Data – Notice the center and ellipse shape of the plot. (Right) Calibrated Data – Center corrected and circular shape with radius equal to magnetic field strength.

ii. 3-D Magnetometer Calibration

Above: Raw Magnetometer data (notice the center and the radii of the readings in each axis)

Page 8: IMU Calibration

Above: Calibrated Magnetometer data (notice the center has been corrected to the origin and the every 2 axis data is almost circular)

Accelerometer: Accelerometer readings gives us [0; 0; 1]*9.8 when no rotation is applied on any axis (roll, pitch or yaw). When rotation is applied;

Gp = Rx(roll)*Ry(pitch)*Rz(yaw)*[0; 0; 1]*9.8 Eqn 1

In general, the accelerometer data plot should give us a sphere centred at the origin with a radius of the gravitational pull, i.e, 9.8. But due to the sensor errors, these readings give us an ellipsoid centered at an offset. To correct these errors, we use the least squares method.

Calibration: The calibration of the accelerometer is similar to that of the magnetometer . Let Gpc be the calibrated reading of the accelerometer;

Gpc = inv(W)*(Gp – V) (where W is the bias and V is the center of the ellipsoid)

Page 9: IMU Calibration

This will fit the ellipsoid data into a sphere and correct the center. To calculate V;

V = (Max(X, Y, Z) + Min(X, Y, Z))/2 (where X, Y, Z represent the accelerometer data)

Eqn 2

This will correct the center of the data plot back to the origin. To calculate W; Following the procedure to calculate the Soft Iron matrix of the magnetometer, W can be calculated in the same way. Let C = (Gp – V) Let R = (C’(*)C’) (where (*) represents the kronecker product) Let A = (inv(W)’*inv(W))

Vec(A) = pinv(R)*(9.8^2) (from Eqn 11 of magnetometer calibration)

Let the matrix inv(W) have the eigenvectors G and eigenvalues U^0.5 and matrix A have the eigenvectors G and eigenvalues O.

Hence, inv(W) = G*O^0.5*inv(G) (from the proof in the magnetometer section)

Eqn 3

From Eqn 2 and Eqn 3;

Gpc = inv(W)*(Gp - V)

The calibrated Accelerometer data is obtained. This was followed in MATLAB for 2 3-axis accelerometers. The results were as such;

i. 3-D Accelerometer Calibration

Page 10: IMU Calibration

Above: Raw Accelerometer Data (notice the center and shape of the figure)

Above: Calibrated Accelerometer data (center corrected to origin and rescaled to form a sphere)

Page 11: IMU Calibration

Calculation of Roll, Pitch and Yaw angles: From the accelerometer readings, roll and pitch can be obtained using the following equation;

Roll = tan inverse of (Gpc(1, 2) / sqrt(Gpc(1, 1)^2 + Gpc(1, 3)^2))

Pitch = tan inverse of (Gpc(1, 1) / sqrt(Gpc(1, 2)^2 + Gpc(1, 3)^2))

By using these two angles and the magnetometer data, yaw can be calculated using the following equation;

Yaw = tan inverse of ((-Bpc(1, 2)*cos(Roll) + Bpc(1, 3)*sin(Roll) ) / (Bpc(1,1)*cos(Pitch) + Bpct(1, 2)*sin(Roll)*sin(Pitch)+ Bpct(1,

3)*sin(Pitch)*cos(Roll)) )

This gives us the three angles corresponding to the rotation of the PCB in the 3 axes. Using these formulae to calculate the angles from the 3-D Accelerometer and Magnetometer data and implementing it in MATLAB, we get;

Above: Roll, Pitch and Yaw angles calculated and plotted using the GUI code. (Fig 1)

Development of GUI:

Page 12: IMU Calibration

Using the Magnetometer and Accelerometer calibrations and angle calculations, a MATLAB based GUI was developed to show the readings, the bias and scale factor matrices for the Magnetometer and also plot the angles.

Angle plot given by Fig 1.

Conclusion: In this report, the method for calibrating the magnetometer and accelerometer sensors is presented. Based on the procedure, it can be separated into two parts; bias correction and scale factor correction, for fitting the ellipsoid to a sphere. After the calibration is performed for the two sensors, the data is used to calculate the orientation angles (roll, pitch and yaw) and heading angle. To evaluate the calibration, data sets from the sensors were used in a MATLAB calibration GUI. The results show that the method corrects the sensor data and calculates orientations without error. It is also noticed that a small error in Yaw is noticed when Roll is at 90 degrees or Pitch at -90 degrees. A Kalman filter can be used, by using a gyroscope we get the angular speed. It is used to correct the error in the angles.

Page 13: IMU Calibration

References:

i. Document: AN4248, Rev. 3 ii. Document: AN4399, Rev. 1

Acknowledgements: I wish to acknowledge Professor Farshad Khorrami and Dr. Prashant Krishnamurty, of the Controls and Robotics Research Lab, for guiding me through the project. I would like to express my great appreciation for all the students in the Controls and Robotics Research Lab for all their valuable suggestion. I would like to offer my special thanks to NYU Polytechnic School of Engineering for giving me the opportunity to work in the Lab.