in this video we're gonna talk about how we can use sensor fusion to estimate an
object's orientation now you may call orientation by other names like attitude
or maybe heading if you're just talking about direction along a 2d plane this is
why the fusion algorithm can also be referred to as an attitude and heading
reference system but it's all the same thing we want to figure out which way an
object is facing relative to some reference and we can use a number of
different sensors to do this for example satellites can use star trackers to
estimate attitude relative to the inertial star field whereas an airplane
could use an angle of attack sensor to measure orientation of the wing relative
to the incoming free air stream now in this video we're gonna focus on using a
very popular set of sensors that you're gonna find in every modern phone and a
wide variety of autonomous systems a magnetometer and accelerometer and a
gyro and the goal of this video is not to develop a fully fleshed-out inertial
measurement system there's just too much to cover to really do a thorough job
instead I want to conceptually build up the system and explain what each sensor
is bringing to the table and a few things to watch out for along the way
I'll also call out some other really good resources that I've linked to below
where you can dive into more of the details so let's get to it
I'm Brian and welcome to a MATLAB Tech Talk when we're talking about
orientation we're really describing how far an object is rotated away from some
known reference frame for example the pitch of an aircraft is how far the
longitudinal axis is rotated off of the local horizon so in order to define an
orientation we need to choose the reference frame that we want to describe
the orientation against and then specify the rotation from that frame using some
representation method and we have several different ways to represent a
rotation and perhaps the easiest to visualize and understand at first is the
idea of roll pitch and yaw and this representation works great in some
situations however it has some widely-known drawbacks in others so we
have other ways to define rotations for different situations things like
Direction cosine matrix YZ and the quaternion now the important thing for
this discussion is not what a quaternion is or how it
is formulated but rather just to understand that these groups of numbers
all represent a three dimensional rotation between two different
coordinate frames the object's own coordinate frame that is fixed to the
body and rotates with it and some external coordinate frame and it's this
rotation or these sets of numbers that were trying to estimate by measuring
some quantity with sensors so let's get to our specific problem let's say we
want to know the orientation of a phone that's sitting on a table so the phone's
body coordinate frame relative to the local northeast and down coordinate
frame we can find the absolute orientation using just a magnetometer
and an accelerometer now a little later on we're gonna add a gyro to improve
accuracy and correct for problems that occur when the system is moving but for
now we'll just stick with these two sensors simply speaking we could measure
the phone's acceleration which would just be due to gravity since it's
sitting stationary on the table and we would know that that direction is up the
direction opposite the direction of gravity and then we can measure the
magnetic field in the body frame to determine north but here's something to
think about the mag field points north but it also points up or down depending
on the hemisphere year-end and it's not just a little bit in north america the
field lines are angled around sixty to eighty degrees down which means it's
mostly in the gravity direction now the reason a compass points north and not
down is that the needle is constrained to rotate within a 2d plane however our
mag sensor has no such constraint so it's going to return a vector that's
also in the direction of gravity so to get north we need to do some cross
products we can start with our measured mag and
Excel vectors in the body frame down is the opposite direction of the
acceleration vector and then East is the cross-product of down and the magnetic
field and finally north is the cross-product of East and down so the
orientation of the body is simply the rotation between the body frame and the
Northeast down frame and I can build the direction cosine matrix directly from
the north east and down vectors that I just calculated let's go check out an
implementation of this fusion algorithm I have a physical IMU it's the MP u 9250
and it has an accelerometer magnetometer and gyro although for now we're not
going to use the gyro I've connected it to an Arduino through I squared C which
is then connected to MATLAB through USB I've pretty much just followed along
with this example from the math works website which provides some of the
functions that I'm using and I've linked to below if you want to do the same but
let me show you my simple script first I connect to the Arduino and the IMU and
I'm using a MATLAB viewer to visualize the orientation and I update the viewer
each time I read the sensors this is a built-in function with the sensor fusion
and tracking toolbox the small amount of math here is basically reading the
sensors performing the cross products and building the DCM and that's pretty
much the whole of it so if I run this we can watch the
algorithm in action
notice that when it's sitting on the table it does a pretty good job of
finding down it's in the positive x-axis and if I rotate it to another
orientation you can see that it follows pretty well with my physical movements
so overall pretty easy and straightforward right
well there are a few problems with this simple implementation I want to
highlight two of them the first is that accelerometers aren't just measuring
gravity they measure all linear accelerations so if the system is moving
around a lot it's gonna throw off the estimate of where down is you can see
here that I'm not really rotating the sensor much but the viewer is jumping
all over the place and this might not be much of a problem if your system is
largely not accelerating like a plane while it's cruising at altitude or a
phone that's sitting on a table but linear accelerations aren't the only
problem even rotations can throw off the estimate because an accelerometer that's
not located at the center of rotation will sense an acceleration when the
system rotates so we have to figure out a way to deal with these corruptions the
second problem is that magnetometers are affected by disturbances in the magnetic
field obviously you can see that if I get a magnet near the IMU the estimate
is corrupted so what can we do about these two problems well let's start with
this magnetometer problem if the magnetic disturbance is part of the
system and rotates with the magnetometer then it can be calibrated out these are
the so-called hard iron and soft iron sources a hard iron source is something
that generates its own magnetic field this would be an actual magnet like the
ones in an electric motor or it could be a coil that has a current running
through it from the electronics themselves and if you tried to measure
an external magnetic field a hard iron source near the magnetometer would
contribute to the measurement and if we rotate the system around a single axis
and measure the magnetic field the result would be a circle that is offset
from the origin so your magnetometer would read a larger intensity in one
direction and a smaller intensity in the opposite direction a soft iron source is
something that doesn't generate its own magnetic field
but is what you would call magnetic you know like a nail that is attracted to a
magnet or the metallic structure of your system this type of metal will bend the
magnetic field as it passes through and around it and the amount of bending
changes as that metal rotates so a soft iron source that rotates with the
magnetometer would distort the measurement creating an oval rather than
a circle so even if you had a perfect noiseless magnetometer it's gonna still
return an incorrect measurement simply because of the hard and soft iron
sources that are near it and your phone and pretty much all systems have both of
those so let's talk about what we can do with calibration if the system had no
hard or soft iron sources and you rotated the magnetometer all around in
for Paiste Radian directions then the magnetic field vector would trace out a
perfect sphere with the radius being the magnitude of the field now a hard iron
source would offset the sphere and a soft iron source would distort it into
some odd shaped spheroid if we can measure this distortion ahead of time we
could calibrate the magnetometer by finding the offset and transformation
matrix that would convert it back into a perfect sphere centered at the origin
this transformation matrix and bias would then be applied to each
measurement essentially removing the hard and soft iron sources this is
exactly what your phone does when it asks you to spin it around in all
directions before using the compass here I'm demonstrating this by calibrating my
IMU using the MATLAB function mag Cal I'm collecting a bunch of measurements
in different orientations and then finding the calibration coefficients
that will fit them to an ideal sphere and now that I have an a matrix that
will correct for soft iron sources and a B matrix that will remove hard iron bias
I can add a calibration step to the fusion algorithm that I showed you
previously and this will produce a more accurate result than what I had before
all right now let's go back to solving the other problem of the corrupting
linear accelerations and one way to address this is by predicting linear
acceleration and removing it from the measurement prior to using it and this
might sound difficult to do but it is possible if the acceleration is the
result of the system actuators you know rather than an unpredictable external
disturbance what we can do is take the commands that are sent to the actuators
and play it through a model of the system to estimate the expected linear
acceleration and then subtract that value from the measurement this is
something that is possible if say your system is a drone and you're flying it
around by commanding the four propellers now if we can't predict the linear
acceleration or the external disturbances are too high another option
is to ignore accelerometer readings that are outside of some threshold from a 1g
measurement if the magnitude of the reading is not close to the magnitude of
gravity then clearly the sensor is picking up on other movement and it
can't be trusted this keeps corrupted measurements from getting into our
fusion algorithm but it's not a great solution because we stopped estimating
orientation during these times and we lose track of the state of the system
again it's not really a problem if we're trying to estimate orientation for a
static object this algorithm would work perfectly fine however often we want to
know the orientation of something that is rotating and accelerating so we need
something else here to help us out what we can do is add a gyro into the mix to
measure the angular rate of the system in fact the combination of magnetometer
accelerometer and gyro are so popular that they're often packaged together as
an inertial measurement unit like I have with my MP you 9250 so the question is
how does the gyro help to start I think it's useful to think about how we can
estimate orientation for a rotating object with just the gyro on its own no
accelerometer and no magnetometer for this we can multiply the angular rate
measurement by the sample time to get the change in angle during that time and
then if we knew the orientation of the phone at the previous sample time we
could just add this Delta angle to it and have an updated estimate of the
current orientation and if the object isn't wrote
then the Delta angle would be zero and the orientation wouldn't change so it
all works out and by repeating this process for the next sample time and the
one after that we're going to know the orientation of the phone over time this
process is called dead reckoning and essentially it's just integrating the
gyro measurement now there are downsides to dead reckoning one you still need to
know the initial orientation before you begin so we need to figure that out and
two sensors aren't perfect they have bias and other high frequency
noises that will corrupt our estimation now integration acts like a low-pass
filter so that high frequency noise is smoothed out a little bit which is good
but the result drifts away from the true position due to random walk as well as
integrating any bias and the measurements so over time the
orientation will smoothly drift away from the truth
so at this point we have two different ways to estimate orientation one using
the accelerometer and the magnetometer and the other using just the gyro and
each way have their own respective benefits and problems and this is where
sensor fusion comes in once again we can use it to combine these two estimates in
a way that emphasizes each of their strengths and minimizes their weaknesses
now there's a number of sensor fusion algorithms that we can use like a
complementary filter or a common filter or the more specialized but common magic
or Mahony filters but at their core every one of them does essentially the
same thing they initialize the attitude either by setting manually or using the
initial results of the Magon accelerometer and then over time they
used the direction of the mag field and gravity to slowly correct for the drift
in the gyro now I go into a lot more detail on this in my video on the
complimentary filter and MathWorks has a series on the mechanics of the common
filter both are linked below but just in case you don't go and watch them right
away let me go over a really high-level concept of how this blending works let's
put our two solutions at opposite ends of a scale that represents our trust in
each one and we can place a slider that specifies which
solution we trust more if the slider is all the way to the left then we trust
our mag and Excel solution 100% and we just use that value for our orientation
all the way to the right and we use the dead reckoning solution 100% when the
slider is in between this is saying that we trust both solutions some amount and
therefore want to take a portion of one and add it to the complimentary portion
of the other by putting the slider almost entirely to the dead reckoning
solution were mostly trusting the smoothness and quick updates of the
integrated gyro measurements which gives us good estimates during rotations and
linear accelerations but we are ever so gently correcting that solution back
towards the absolute measurement of the mag in Excel to remove the bias before
it has a chance to grow too large so these two approaches complement each
other now for the complimentary filter you as the designer figure out manually
where to place this slider how much you trust one measurement over the other but
with a common filter the optimal gain or the optimal position of the slider is
calculated for you after you specify things like how much noise there is in
the measurements and how good you think your system model is so the bottom line
is that we're doing some kind of fancy averaging between the two solutions
based on how much trust we have in each of them now if you want to practice this
yourself the MATLAB tutorial I used earlier goes through a common filter
approach using the function a hrs filter and that's where I'm going to leave this
video in the next video we're gonna take this one step further and add GPS and
show how our IMU and orientation estimate can help us improve the
position that we get from the GPS sensor so if you don't want to miss that or
other future Tech Talk videos don't forget to subscribe to this channel also
if you want to check out my channel control system lectures I cover more
control theory topics there as well I'll see you next time