imufilter results seem incorrect

Hi,

I used an IMU to collect acceleration and gyroscope that are under the sensor's local coordinate system. The local acceleration and gyroscope were low-pass filtered with cut-off frequency of 10Hz. Then I used imufilter to calculate the rotation matrix and transformed the local acceleration. The code is as follows.

% Read csv file
data = readmatrix(data_path);
timestamp = data(:, 1);
acc = data(:, 2:4); % acc size: N * 3
gyro = data(:, 5:7);

% Calculate orientation
fuse = imufilter('SampleRate', 50, 'OrientationFormat', 'Rotation Matrix');
[orientation, ] = fuse(acc, gyro); % orientation size: 3 * 3 * N

% Transform acc
[rows, cols] = size(acc);
earth_acc = zeros(rows, cols);
for n = 1:rows
  earth_acc(i, :) = acc(i, :) * orientation(:, :, i);
end

Then the transformed acceleration seems to be incorrect. To be more specific, the local acceleration looks like the following picture, where X-axis is frame, and Y-axis is acceleration (m/s^2).

However, the transformed acceleration looks like the following picture. To me, it just looks like noise.

Can you please help me to figure out what is going on with the code? Thank you!

[Paul]

Hi Yunda,

Can you attach the file data_path to your Question using the paper clip icon in the Inser section of the editor ribbon?

[Yunda Liu]

Hi Paul,

I've attached to the data.csv to the original post. Please let me know if you have any questions.

[Rena Berman]

(Answers dev) I have restored this attachments to this question as best as possible but one might need to be recreated.

Answer 1

[Paul]

I think the gyro data is in deg/sec, but the imufilter wants that input in rad/sec. Check those units.

data = readtable('data.csv');
data = table2array(data);
timestamp = data(:, 1);
acc = data(:, 2:4); % acc size: N * 3
gyro = data(:, 5:7);

Plot the gyro data. Those peaks seem awfully large if in rad/sec

figure
plot(timestamp,gyro)

figure
plot(timestamp,acc),grid

Scale the gyro readings assuming that gyro is in deg/sec

% Calculate orientation
fuse = imufilter('SampleRate', 50, 'OrientationFormat', 'Rotation Matrix');
[orientation, ] = fuse(acc, gyro*pi/180); % orientation size: 3 * 3 * N
% Transform acc
[rows, cols] = size(acc);
earth_acc = zeros(rows, cols);
for n = 1:rows
  %earth_acc(i, :) = acc(i, :) * orientation(:, :, i);
  earth_acc(n, :) = acc(n, :) * orientation(:, :, n);
end

Looks more reasonable.

figure
plot(timestamp,earth_acc),grid

[Yunda Liu]

Thank you so much for the help!

[Yunda Liu]

Hi Paul,

I examined the code, and I think there are a few more things I need to change and I want to double check with you.

First, as the document of imufilter says, "Orientation that can rotate quantities from a global coordinate system to a body coordinate system, returned as quaternions or an array". In the code above, because acc is in the body coordinate system and we want to rotate acc to a global coordinate system, we need to calculate the inverse of the orientation first before multiplying it with acc.

Second, in coordinate transformation, the order of the multiplication is important and orientation must be on the left of the multiplication. In other words, we must do

inv(orientation(:, :, n)) * acc(n, :).' 

instead of

acc(n, :) * orientation(:, :, n)

Third, suppose I have N data points, and each data point was collected at time t = 1, 2 .. N. If I understand correctly, imufilter calculates the orientations of the body, relative to its initial orientation. In other words, imulfilter treats that the coordinate system at t=1 as the global coordinate system and rotate the acceleration to the coordinate system at t=1. My question is in this case, shouldn't the rotation matrix at t=1 be [[1,0,0];[0,1,0];[0,0,1]]? Becase at t=1, the data point does not need to be rotated and the rotation matrix should be identity.

[Paul]

My understanding of the documentation is the same as yours. So if we want to store earth_acc by rows, then we'd have

earch_acc(n,:) = ( inv(orientation(:, :, n)) * acc(n, :).' ) .' ;

But the inverse of a direction cosine matrix is its transpose

earch_acc(n,:) = ( orientation(:, :, n).' * acc(n, :).' ) .' ;

And using the rule for the transpose of a product yields what you had:

earth_acc(n,:) = acc(n, :) * orientation(:, :, n);

As to the question about the initial value of orientation, won't be able to check that unless you attach your data to the question again (it seems to have been lost when we had the problem with not being able to open this Question).

Using an example from the doc

load 'rpy_9axis.mat' sensorData Fs

accelerometerReadings = sensorData.Acceleration;
gyroscopeReadings = sensorData.AngularVelocity;
fuse = imufilter('SampleRate',Fs,'OrientationFormat', 'Rotation Matrix');
orientation = fuse(accelerometerReadings,gyroscopeReadings);
orientation(:,:,1)

ans = 3×3

0.9631 -0.0022 0.2690 -0.0289 0.9933 0.1115 -0.2674 -0.1152 0.9567

That does seem strange. I don't have an explanation.

[Yunda Liu]

Hi Paul,

As to the question about the initial value of orientation, we both assumed that if we don't specify reference frame in imufilter, imufilter calculates the orientations of the body, relative to its initial orientation. However, I talked with the technical support from Matlab. And this is their answer.

• The reference frame is always known and taken as the North-East-Down frame by default if no reference frame argument, ‘ReferenceFrame’ is provided to the ‘imufilter’ object.
• There are 2 options you can select to set your reference frame, particularly – ‘NED’ or ‘ENU’. The following implementation will set the reference frame as the ‘NED’ frame:

    fuse = imufilter('SampleRate',Fs,'DecimationFactor',decim);

• OR

    fuse = imufilter('SampleRate',Fs,'DecimationFactor',decim, 'ReferenceFrame', 'NED');

Therefore, the orientation obtained from imufilter is always relative to NED frame, not the sensor's initial orientation, even though my sensor was not aligned with NED at the beginning. If we want to calculate the orientation relative to the initial orientation, I think we need some more calculation. Suppose at t = 1, the rotation matrix is R0; at t = n, the rotation matrix is Rn. Then at t = n, the orientation relative to the initial orientation should be transpose(R0) * Rn. Is that correct?

[Paul]

It is true that I assume a) that imufilter calculates orientation relative to the the inial orientation. But it's also true that I assume b) that imufilter assumes that the inital orientation is NED (or whatever is specified by 'ReferenceFrame'). I explained my reasoning for assumption (b) in this comment to your other Question.

The response from tech support doesn't make sense IMO. Based on my reading of the doc, imufilter has no way of knowing how your IMU is oriented relative to NED (or 'ReferenceFrame') at t = 0.

As I explained in that linked comment, it seems to me that imufilter makes a fundamental assumption that the IMU at t = 0 is aligned with NED (or 'ReferenceFrame').

However, based on the example above, imufilter apparently doesn't assume the IMU is exactly aligned with NED at t = 0, otherwise orientation(:,:,1) would be eye(3).

In the example from the doc

load 'rpy_9axis.mat' sensorData Fs
accelerometerReadings = sensorData.Acceleration;
gyroscopeReadings = sensorData.AngularVelocity;
fuse = imufilter('SampleRate',Fs,'OrientationFormat', 'Rotation Matrix');
orientation = fuse(accelerometerReadings,gyroscopeReadings);
orientation(:,:,1)

ans = 3×3

0.9631 -0.0022 0.2690 -0.0289 0.9933 0.1115 -0.2674 -0.1152 0.9567

[rotationAng1 rotationAng2 rotationAng3] = dcm2angle(orientation(:,:,1));
180/pi*[rotationAng1 rotationAng2 rotationAng3]

ans = 1×3

-0.1288 -15.6022 6.6504

somehow imufilter comes up with very large angles for the initial orientation.

Rerun the example, but with a nearly ideal IMU.

fuseideal = imufilter('SampleRate',Fs,'OrientationFormat','Rotation Matrix', ...
'AccelerometerNoise',eps, ...
'GyroscopeNoise',eps, ...
'GyroscopeDriftNoise',eps, ...
'LinearAccelerationNoise',eps, ...
'LinearAccelerationDecayFactor',eps, ...
'InitialProcessNoise',1e-10*eye(9));
orientation = fuseideal(accelerometerReadings,gyroscopeReadings);
orientation(:,:,1)

ans = 3×3

0.9656 -0.0026 0.2600 -0.0279 0.9932 0.1133 -0.2586 -0.1167 0.9589

We see the initial orientation changes, albeit by not much, but a change nevertheless. Why would the initial orienation estimate change based on the modeled quality of the measurements? It seems to me that the initial orientation estimate would be independent of any measurement.

What happens if the IMU measurements are zero at t = 0?

reset(fuseideal);
orientation = fuseideal([0 0 0;accelerometerReadings],[0 0 0;gyroscopeReadings]);
orientation(:,:,1)

ans = 3×3

1 0 0 0 1 0 0 0 1

Somehow the initial orienation is being determined from the initial measurements (both initial accel and gyro measurements have to be 0, one or the other non-zero changes the result).

Apparently the IMU error model in imufilter does not affect the initialization with zero initial measurements.

reset(fuse)
orientation = fuse([0 0 0;accelerometerReadings],[0 0 0;gyroscopeReadings]);
orientation(:,:,1)

ans = 3×3

1 0 0 0 1 0 0 0 1

This is all very mysterious to me. If I get motivated, I'll try to look into the source documentation linked from the doc page.

I think tech support should be able to explain how the imufilter algorithm develops the initial orientation based on the input IMU measurements and the imufilter properteis.

[Yunda Liu]

Hi Paul,

Thank you so much for the comments. It is really helpful.

I looked at the documentation. The documentation says that during the first iteration (of orientation of prediction), the orientation estimate, q−, is initialized by ecompass with an assumption that the x-axis points north. I personally think this is why imufilter makes a fundamental assumption that the IMU at t = 0 is aligned with NED (or 'ReferenceFrame').

As for why imufilter doesn't assume the IMU is exactly aligned with NED at t = 0, I sent an email to the tech support and hopefully we can get some insights from them.

[Paul]

Hi Yunda,

Are you referring to the documentation at https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/tree/master/docs

or the documentation for imufilter? For the latter, I did a ctrl-f and didn't seen any reference to ecompass on the imufilter doc page, other than the link to it in the See Also section.

How can ecompass be used for initialization? The second input to ecompass is magnetometerReading, which hasn't been defined, much less used, anywhere in this thread. Maybe imufilter assumes the IMU is initially located at some default location on the earth that defines the magnetic field strength?

[Yunda Liu]

Hi Paul,

I am referring to this webpage (shown in the picture below). I am a bit confused about how ecompass was used to initilize rotation as well. But at least it explained why the default orientation is NED.

[Yunda Liu]

Hi Paul,

I received the answers from the tech team. This is their answer.

‘imufilter’ calculates orientation relative to the reference frame specified in the ‘imufilter’ system object. If no argument is specified, the reference frame is selected as NED. The NED reference frame is defined such that the North and East axes form a plane tangent to the Earth's surface at any position at any given time. Meaning the ‘orientation’ will always be with respect to the same NED reference frame at t = 0 as it would be at t = k.

The standard way to express the gravitational vector in a NED frame anywhere on Earth is expressed as:

gNED = [0 0 9.8] % m/s^2

Meaning, that at any given point in time when your sensor frame exactly aligns with the NED frame, it should theoretically experience a downward acceleration of 9.8 m/s^2 in the z-direction (Down according to the NED frame) and 0 m/s^2 in the x and y direction (North and East respectively). When this happens, the rotation matrix should be an identity matrix.

Please note that since you are using a Kalman filter, you won't immediately get an identity matrix, rather it will converge over time and is the expected behavior.

The above information on getting an identity matrix when the sensor and reference frame align is true for theoretical purposes. When you deal with real-world data or sensors, it is subjected to noise, and measurement errors, and not to forget gravity is different at each point on Earth. Thus, when you try to align your sensor frame to the NED frame, you will get a matrix close to an identity matrix, but it won’t be an identity matrix exactly. However, for theoretical and calculation purposes, we treat gravity to be the same as ‘gNED’.

I would also like to refer you to some documentation that you will find helpful:

• Express Gravitational Vector in Body Frame

https://www.mathworks.com/help/fusion/gs/spatial-representation-coordinate-systems-and-conventions.html#responsive_offcanvas

• Orientation

https://www.mathworks.com/help/fusion/ug/estimate-orientation-through-inertial-sensor-fusion.html#IMUOrientationExample-1

• Estimate Orientation from IMU data (please don’t use this example to get exact identity matrices as it is based on real measured data from IMU prone to noise and other errors)

https://www.mathworks.com/help/fusion/ref/imufilter-system-object.html#d124e150663

• References used for creating the algorithm:

https://www.mathworks.com/help/fusion/ref/imufilter-system-object.html#References

[Paul]

@Yunda Liu

Thanks for finding this line from the imufilter doc page:

During the first iteration, the orientation estimate, q−, is initialized by ecompass with an assumption that the x-axis points north.

I have two concerns about that statement:

1. I don't see how ecompass can be used to determine orientation using only the information available within imufilter, i.e., the accelerometer readings and the gyro readings. As shown on ecompass, it takes in accelerometerReading and magnetometerReading. The accelerometerReading is supposed to be the gravity vector (to within a scale factor) resolved in sensor coordinates. The magnetomemterReading is the magnetic field vector (to within a scale factor), also resolved in sensor coordinates. The magnetic field vector is assumed to lie in the ND plane.

Let's illustrate with an example. If we take statement 1 literally the x-axis of the IMU has to be pointed north. So the only angular degree of freedom it has is a roll angle around that x-axis. Let's construct a DCM based on a roll angle from NED to IMU coordinates that left multiplies a column vector resolved in NED coordinates to yield that same vector resolved in IMU coordinates.

DCMned2imu = angle2dcm(0,0,pi/3,'ZYX')

DCMned2imu = 3×3

1.0000 0 0 0 0.5000 0.8660 0 -0.8660 0.5000

The gravity vector in NED is

gNED = [0 0 9.8];

The magnetic field vector in NED must lie in the ND plane

mNED = [1 0 1];

Resolve both in IMU coordinates as if they were measured by the device

gIMU = (DCMned2imu*gNED.').';
mIMU = (DCMned2imu*mNED.').';

Call ecompass to estimate the orientation, which yields the same result we started with.

orientation = ecompass(gIMU,mIMU,'rotmat')

orientation = 3×3

1.0000 0 0 0 0.5000 0.8660 0 -0.8660 0.5000

The point here is that ecompass requires both inputs to be resolved in the IMU coordinate frame. However, imufilter has no input for the magnetic field reading in IMU coordinates, so I don't understand how ecompass, as documented, can be used to determine the initial orientation based only on the accelerometer readings that are input to imufilter.

2. As will be shown below it appears that imufilter doesn't require that the x-axis of the IMU be pointed north, i.e., in the direction of the N-axis, only that it lies in the ND plane. To be fair, "points north" could be interpreted to mean as having no east component, and this interpretation seems to be the operative one in imufilter.

To be sure, using the 'ZYX' rotation sequence, one could compute IMU pitch and roll angles from a gravity vector resolved in IMU coordinates (the yaw angle is arbitrary), but I don't see how that's done with ecompass.

BTW, consistent with other functionality in the SFT Toolbox, ecompass assumes the accelerometer readings are the negative of what a real accelerometer would measure. That is, an accelerometer experiencing zero net external force acting on it would output the negative of the gravity vector, whereas ecompass (and imufilter) assume it outputs the gravity vector.

Here are some examples of imufilter to illustrate. The function below constructs truth motion of the IMU, uses truth motion to construct IMU measurements, and the uses those as inputs to an imufilter.

Case 1: set the initial yaw, pitch, and roll angles of the IMU to zero.

offsets = [0 0 0];
tempfunc(offsets);

The imufilter seems to do very well. Intrestingly enough the largest error at t = 0 is in the yaw angle, even though IMU filter doc page implies that imufilter assuems the IMU is pointed northward.

Case 2: Assume zero initial yaw angle, non-zero initial pitch and roll angles

offsets = [0 pi/6 pi/5];
tempfunc(offsets)

Again the pitch and roll angles seem to be good, with a small error in the yaw angle.

Case 3: All three non-zero initial angles

offsets = [pi/3 pi/6 pi/5];
tempfunc(offsets)

We clearly see that, in accordance with the doc page, imufilter doesn't know anything about an initial yaw angle.

Summary (based on this comment):

a. I still don't understand how imufilter is iniatializing the orientation, and I don't understand how it could use ecompass (as ecompass is documented).

b. Regardless, the imufilter orientation seems to be able to capture inital pitch and roll angles, but not initial yaw angle. I need to think more about whehter or not this property is important.

c. Not discussed above, but as I understand it imufilter assumes the IMU is experiencing zero net force, and is therefore not accelerating relative to an inertial frame. Does this limit the applications for which imufilter is useful?

d. The accelerometerReading input to imufilter is the negative of what a real acceleromter would read under zero net applied force.

function tempfunc(offsets)

% Sample frequency and time vector
Fs = 100;
t  = (0:100).'/Fs;

% sinusoidal yaw, pitch, and roll angles of the IMU, and their rates (rad,
% rad/sec)
amplitude  = [.3 .5 .75];
frequency  = 2*pi./[1 2 3];
angles     = amplitude.*sin(frequency.*t) + offsets;
anglerates = frequency.*amplitude.*cos(frequency.*t);

% DCM from NED to IMU 
DCMned2imu = angle2dcm(angles(:,1),angles(:,2),angles(:,3),'ZYX');

% angular velocity vector of IMU (rad/sec)
temp = permute(anglerates,[2 3 1]);
angularvelocity = ...
    pagemtimes(angle2dcm(0*angles(:,1),  angles(:,2),angles(:,3),'ZYX'),flipud(temp.*[1;0;0])) + ...
    pagemtimes(angle2dcm(0*angles(:,1),0*angles(:,2),angles(:,3),'ZYX'),temp.*[0;1;0]) + ...
    flipud(temp.*[0;0;1]);
angularvelocity = ipermute(angularvelocity,[2 3 1]);

% imufitler, assuming close-to-ideal IMU
fuseideal = imufilter('SampleRate',Fs,'OrientationFormat','Rotation Matrix', ...
'AccelerometerNoise',eps, ...
'GyroscopeNoise',eps, ...
'GyroscopeDriftNoise',eps, ...
'LinearAccelerationNoise',eps, ...
'LinearAccelerationDecayFactor',eps, ...
'InitialProcessNoise',1e-10*eye(9));

% IMU readings. The accelerometer reading is the gravity vector resolved in
% IMU coordinates, opposite of a real accelerometer
% gravity vector resolved in NED
gNED                  = [0 , 0 , 9.8];
accelerometerReadings = ipermute(pagemtimes(DCMned2imu,gNED.'),[2 3 1]);
gyroscopeReadings     = angularvelocity;

% estimate the orientation and extract the angles
orientation = fuseideal(accelerometerReadings,gyroscopeReadings);

fuzeangles = 0*angles;
[fuseangles(:,1),fuseangles(:,2),fuseangles(:,3)] = dcm2angle(orientation,'ZYX');

% plot comparison
figure;plot(t,180/pi*[angles(:,1) fuseangles(:,1)]),grid,ylabel('Yaw (deg)')
figure;plot(t,180/pi*[angles(:,2) fuseangles(:,2)]),grid,ylabel('Pitch (deg)')
figure;plot(t,180/pi*[angles(:,3) fuseangles(:,3)]),grid,ylabel('Roll (deg)')
legend('True','IMUFilter')

end