Implement IMU

[whymarrh]

(What is an IMU? Inertial measurement unit - Wikipedia)

We will need add an IMU if we want to get better bearing measurements.

[whymarrh]

@arandell93 thoughts? What do device(s) do you have at your disposal?

[arandell93]

What do device(s) do you have at your disposal?

The device I have is only a 3-axis accelerometer. So that won't do.

Based on some reading I have done (in the past, so no references available) we will need at minimum a magnetometer to improve our bearing reading. In addition to this, it is common to use an accelerometer in tandem with a magnetometer as a method of 'filtering' out the horizontal (parallel to the ground, therefore normal to the gravity vector) component of a 3-axis magnetometer to decrease the likelyhood of interference.

With that said, we may as well get a 9-axis IMU (accel, mag, gyro) since we will likely have a use for a gyro eventually, and the cost is only a marginal increase.

The Pololu AltIMU-10 is what I purchased for EER last year and we were able to communicate with it and see some sensible numbers. It also includes a pressure sensor so it's even more bang for the buck. It uses I2C to communicate, where each device on the board (gyro, accel, mag, barometer) has their own address and set of config/data registers.

[arandell93]

@FifoIronton please place the required information for @whymarrh to implement the IMU here.

[FifoIronton]

We're using an Adafruit 1714. There's a pretty neat library written by some Adafruit guy and released with a BSD library. Look here. The code is really well commented, but if you can't get what you want out of it you can look at the code for the two chips provided by pololu here and here, which are released under an MIT license.

We pretty much want to recreate the pitchrollheading example that adafruit gives, rather than the raw values out of the registers. It looks at receiving pitch, roll, and heading, all good things.

[arandell93]

That pitchrollheading example looks about right. They are using the magnetometer for heading (as opposed to gyro). We can use this as a jumping off point and should allow us to tune a rough control system when used in tandem with the GPS.

Eventually we may want to integrate the gyro for more accurate heading data but we can defer that to a later date and a separate issue.

[whymarrh]

@arandell93 feel free to open up a separate issue for that if you're clear on the separation

[arandell93]

For reference in coordinate transformations and general important navigation things:

We will be obtaining the x,y,z value of magnetic flux density (in Gauss, B, or Tesla, T) from the magnetometer on the IMU and will be converting it to spherical coordinates to make things easier.

From there, we are only really concerned with the θ which represents the angle of the IMU with respect to magnetic north and thus we will be calculating our heading from that.

Once implemented, we will need to determine which direction the IMU is mounted with respect to the magnetometer readings so that we can offset the angle. We can do this by aligning the boat to known magnetic north and determining the angle given to us by the IMU. This angle offset will then be applied to all calculations of the boat's heading.

[CalvinGregory]

Cartesian coordinates are perfectly fine, and easier to work with than spherical. Does the imu have a quaternion output option? They're a bit harder to understand but we wouldn't have to worry about gimble lock or other singularities. Magnetometer units don't really matter.

[arandell93]

My main reasons for wanting to use spherical coordinates are:

• The θ is what we will be using for heading
• Having a reading of the magnitude of the 3 dimensional vector allows us to more easily identify and reject anomalous data (such as spikes in magnetic field strength due to spinning up/slowing down the motors)
• It is more intuitive for testing/tuning because it has a more significant physical meaning to us based on a readout

[CalvinGregory]

The reasons I'd like to avoid spherical coordinates if possible:

• The math gets hard. Spherical coordinate derivatives are not fun to solve.
• I believe Euler angles (pitch roll yaw) are more intuitive to read and debug than spherical coordinates.
• The transformations between Euler, quaternion, spherical are each doable in a single function call so we're not making our work that much easier once data leaves the imu. Would it not be better to use the sensor's native coordinate frame?

The state estimation system is going to have to be an abstraction on top of the sensor data anyway and we can transform those outputs to whatever coordinate type we desire. I don't think the ability to read the heading as a single value off the imu is essential long term considering we won't be using that imu reading as a direct heading reading long term.

[CalvinGregory]

AHRS_linearization.m

This script is generating the state space equations to predict the system's behavior under a given set of conditions (states).

clearvars
close all
clc
%%
syms T q0 q1 q2 q3 wmx wmy wmz bwx bwy bwz bx by bz g real

% be = [0.24510 0.00231 -0.38636]'; % gauss
% ge = [0 0 -9.80358]'; % m/s^2
be = [bx by bz]';
ge = [0 0 g]';

q = [q0 q1 q2 q3];
wm = [wmx wmy wmz]';
bw = [bwx bwy bwz]';

x = [q';bw];
u = wm;

Q4x3 = skewql(q);
Q4x3 = Q4x3(:,2:4);
Rq = q2rotm(q);

f = [0.5*Q4x3*(wm-bw);
     zeros(3,1)];

h = [-Rq'*ge;
     Rq'*be];

% Linearize model
n = size(f,1);
m = size(h,1);

% Jacobian of f
for i = 1:n
    for j = 1:n
        F(i,j) = diff(f(i),x(j));
    end
end
% F
% Jacobian of h
for i = 1:m
    for j = 1:n
        H(i,j) = diff(h(i),x(j));
    end
end

G = eye(7);

Phi = eye(n) + F*T + (F*T)^2/2;
% Q
% Qd = G*Q*G'*T^2;
% R

[CalvinGregory]

AHRS.m

This script is running the model produced in AHRS_linearization.m on a 9-DOF imu dataset.

% Run DatasetExtraction.m to import ADIS IMU messages. 
close all
clc
%% Initialization
msg = parseMsg(adisMsgs{1}, start);
magBias = [0 0 -0.12]';
q = msg.quat;
% bnoise = msg.bias;
bnoise = [0 0 0]';
xh = [q';bnoise];

P = eye(7);

t = 0;

figure(1)
axis([-1 1 -1 1 -1 1]);
grid on
hold on
xlabel('X');
ylabel('Y');
zlabel('Z');
adis = scatterbotsSE3([0 0 0]', q, 'init', [], 0.3, 'b');
estimate = scatterbotsSE3([0 0 0]', xh(1:4)', 'init', [], 0.2, 'r');

gnoise = 0.16; % °/s rms
gnoise = (gnoise*(pi/180))^2;
ng = diag([0 1 1 1])*gnoise;
nbg = 10*gnoise*eye(3);
Q = [ng zeros(4,3);
    zeros(3,4) nbg];
G = eye(7);
anoise = 1.5; % mg
anoise = (anoise/1000*9.80665)^2; % m/s^2
na = eye(3)*anoise;

bnoise = 0.45; % mgauss
bnoise = bnoise/1000; %gauss
nb = eye(3)*bnoise;
R = [na zeros(3);
     zeros(3) nb];

global ge be

%% Loop

angles = [];
angles2 = [];
angles3 = [];
magEarth = [];
magSensor = [];
for i = 1:size(adisMsgs,1)
    % Import sensor values at time step k.
    msg = parseMsg(adisMsgs{i},start);
    t = [t;msg.time];
    T = t(end) - t(end-1);
    u = msg.gyro;
    correctedMag = msg.mag - magBias;
    ym = [msg.accel;correctedMag];
%   ym = ym(1:3); % test accelerometer
%   ym = ym(4:6); % test magnetometer

    %sanity check
    thx = atan2(-msg.accel(2)/ge(3),-msg.accel(3)/ge(3)); 
    thy = atan2(msg.accel(1)/ge(3),-msg.accel(2)/(ge(3)*sin(thx)));
    angles = [angles;[thx thy]];

    [thz, thy, thx]=quat2angle(msg.quat,'ZYX');
%   angles2 = [angles2; msg.euler(1:2)'];
    angles2 = [angles2; [thx thy]];
    angles3 = [angles3; msg.euler(1:2)'];

    Rq = q2rotm(msg.quat);
    magEarth = [magEarth; (Rq'*be)'];
%   magEarth = [magEarth; (Rq*be)'];
    magSensor = [magSensor; correctedMag'];

    % Time step k(-)
    yh = h(xh);
%   yh = yh(1:3); % test accelerometer
%   yh = yh(4:6); % test magnetometer

    % Time step k(+)
    Hk = H_lin(xh);
%   Hk = Hk(1:3,:); % test accelerometer
%   Hk = Hk(4:6,:); % test magnetometer
    K = P*Hk'/(R + Hk*P*Hk');
%   K = P*Hk'/(R(1:3,1:3) + Hk*P*Hk'); % test accelerometer
%   K = P*Hk'/(R(4:6,4:6) + Hk*P*Hk'); % test magnetometer
    xh = xh + K*(ym-yh);
    P = (eye(7) - K*Hk)*P;

    % Time step k+1(-)
    phi = Phi(xh, u, T);
    Qd = G*Q*G'*T^2;
    P = phi*P*phi' + Qd;
    xhdot = f(xh,u);
    xh = xh + xhdot*T;
    xh = [quatnormalize(xh(1:4)')';xh(5:7)];

    % Visualization
%   R_sb = [1 0 0;
%           0 -1 0;
%           0 0 -1];

    scatterbotsSE3([0 0 0]', msg.quat, 'update', adis, 0.3, 'b');
    scatterbotsSE3([0 0 0]', xh(1:4)', 'update', estimate, 0.2, 'r');
%   scatterbotsSE3([0 0 0]', quatmultiply(xh(1:4)',rotm2quat(R_sb)), 'update', estimate, 0.2, 'r');
    drawnow
end
t = t(2:end);
%% Analysis

figure(1)
subplot(3,1,1)
plot(t,angles(:,1))
subplot(3,1,2)
plot(t,angles2(:,1))
subplot(3,1,3)
plot(t,angles3(:,1))
movegui(1,'northwest')

figure(2)
subplot(3,1,1)
plot(t,angles(:,2))
subplot(3,1,2)
plot(t,angles2(:,2))
subplot(3,1,3)
plot(t,angles3(:,2))
movegui(2,'northeast')

figure(3)
plot(t,magEarth(:,1),'b',t,magSensor(:,1),'r--')
title('Magnetometer X')
movegui(3,'southwest')

figure(4)
plot(t,magEarth(:,2),'b',t,magSensor(:,2),'r--')
title('Magnetometer Y')
movegui(4,'south')

figure(5)
plot(t,magEarth(:,3),'b',t,magSensor(:,3),'r--')
title('Magnetometer Z')
movegui(5,'southeast')

[CalvinGregory]

The parts pertaining to tonight's discussion are mostly found in the first comment (AHRS_linearization.m). The line h = [-Rq'*ge;Rq'*be]; in the first file is calculating the predicted sensor output for the accelerometer and magnetometer where "ge" is the local gravity vector and "be" is the local earth magnetic field vector. "Rq" is the transformation matrix between the vehicle's body frame and the tangent frame (earth).

The way the kalman filter works is by using an estimated system state (i.e. position, velocity, orientation, etc.) to predict the sensor outputs. That prediction is then compared to the real sensor values, and depending on how far off the prediction was, the system state estimate is updated. So in summary, the system is predicting the magnetometer reading based on the vehicle's estimated orientation and the earth's local magnetic field (in xyz) then comparing that estimate to the real sensor values to see how much to correct itself in the next time step.

[arandell93]

Some software to do magnetic field grid thingys. https://www.ngdc.noaa.gov/IAGA/vmod/igrf.html

[CalvinGregory]

That's neat. Here's another way to do it. http://geomag.nrcan.gc.ca/calc/mfcal-en.php

And this page describes the most common ways of representing magnetic fields. http://geomag.nrcan.gc.ca/mag_fld/comp-en.php

[whymarrh]

The two Adafruit libraries for the two chips in the 9DOF breakout board (as per Nick's example above):

• adafruit/Adafruit_L3GD20_U
• adafruit/Adafruit_LSM303DLHC