11.6: Sensor Fusion vs. Single Sensor

State Flow and Initialization

For the mini-Segway, an MPU 6050 sensor board is connected to the I2C pins of the Arduino MKR 1000 Wi-Fi board. The MPU 6050 sensor set is a 6-DOF (degree of freedom) inertial measurement unit used to detect sensor acceleration and angular velocity in all three dimensions. It incorporates both a three-axis gyroscope and a three-axis accelerometer. Its measured signals can be accessed through the I2C communication bus. We can read the sensor data in MATLAB/Simulink by proper configuration of the MPU 6050.

In addition, the MPU 6050 needs to be initialized once powering up to have repeatable operation. For initialization, the I2C address of the slave with the write command needs to be sent, followed by the internal register number to write to. Note that MATLAB/Simulink provides the I2C Master Write block capable of writing data to an I2C slave device or a device register as shown in Figure B.73. A slave device is a passive component in a communication system that operates under the direction of a single master device (e.g., Arduino MKR 1000). The slave device address of MPU 6050 can be found either from its data sheet or by typing the command as shown in Figure B.74 in MATLAB after the host computer is connected to the MPU 6050 through mini-Segway control hardware.

The MPU 6050 provides a programmable full-scale range of \(\pm 2\), \(\pm 4\), \(\pm 8\), and \(\pm 16\) g for acceleration measurements and full-scale range of \(\pm 250\), \(\pm 500\), \(\pm 1000\), and \(\pm 2000\) degree/second for \(x\)-, \(y\)-, and \(z\)-axis angular rate sensors for these rate gyros. During the initialization process, the ranges of \(\pm 4\ \)g and \(\pm 1000\) degree/second, respectively, were selected by writing commands to these associated registers, as shown in Figure B.75.

The specific register to read and write is in the data sheets. However, once the MPU 6050 is configured and running, you can read the associated data. The gyro data starts at register 0x3b.

This model uses a State Flow chart, a software package extending MATLAB and Simulink for modeling logic and event-driven systems. When active, the contents of the selected state are executed sequentially. The entry action is executed immediately upon entering this state. After the entry phase, and for as long as this state is active, the commands in the “During” section of the State Flow model in Figure B.76 are executed. The code for “During” can be thought as the body of a “for” loop in software. Then, as the state becomes inactive, the exit commands are executed. The Simulink function is called from within State Flow charts. Note that a Simulink function is a Simulink subsystem defined to write and read data for MPU 6050 in the mini-Segway Simulink model.

The developed MPU 6050 Simulink model is shown in Figure B.76. One can study the model based on the descriptions above. Please do not change any parameters in this block. In addition, the register map and data sheet of MPU 6050 can be found in reference [8] of Chapter 6.

With model outputs defined, from the datasheet, the sensitivity scaling factor can be found for the gyroscope, the accelerometer, and the sensor temperature based on the selected full sensing range. An example for the gyroscope scaling factor is shown in Figure B.77.

Acceleration and angular velocity can be calculated by dividing sensor raw data by their associated sensitivity scaling factor. One example for rate gyro is shown in Figure B.76. A Simulink Demux block was used to split signals to associated three axes, respectively.

As introduced in Section 6.5, mini-Segway pitch angle can be calculated from either accelerometer signals (\(a_x\) and \(a_z\)) or the gyro signal (\(ω_g\)). Using the accelerometer signals in \(x\) and \(z\) directions, the mini-Segway pitch angle can be calculated by

\[θ_a=\mathrm{atan}\left(\dfrac{a_x}{a_z}\right) \label{B.11}\]

Using the gyro signal, the angle can be calculated from angular rate by a discrete integration transfer function, as below.

\[θ_g=\dfrac{T_sz}{z-1}\mathrm ω_g \label{B.12}\]

where \(T_s\) is the sample period.

Exercise B6.2

Connect the rate gyro outputs to the scope and rotate the mini-Segway along each axis manually so that you can associate the sensor output with those of mini-Segway. Describe your findings about roll, pitch, and yaw angles in the report.The direction of the three-orthogonal coordinate system is shown in Figure B.78. When you rotate or move your mini-Segway, keep the arrow of the \(x\)-axis point to the forward position.

The reference model for completing Exercise B6.2 is shown in Figure B.79.

Sensor Fusion for Calculating Pitch Angle

The pitch angle can be calculated using the formulae in equations (B.11) and (B.12). However, the pitch angle calculated based upon accelerometer signals is accurate only at low frequency because the calculated pitch angle becomes very noisy at high frequency. The pitch angle based on the gyroscope signal is accurate only at high frequency. This is because the integration operation leads to the calculated value drift at low frequency due to rate gyro signal bias. In this lab section, sensor fusion technology will be introduced in Simulink by the seven steps below. For this sensor fusion application, we will adopt the complementary filter technique (see Section 6.5). The basic idea behind this technology is to combine the two calculated outputs from both the accelerometer signals and the gyroscope signal, utilizing the high- and low-pass filters, to obtain a good estimate of the mini-Segway pitch angle.

1. Choose Trigonometric Function from the Simulink “Math” library and select atan2 for function in the parameter dialog window.
2. Connect this block to X acc and Z acc, resulting in the calculated pitch angle based on accelerometer signals.
3. Add a gain block to convert the unit of output, obtained in Step 2, from radian to degree so that the unit is consistent with the output from gyro.
4. Run the model in the external mode with infinite time. Describe the difference of pitch angle from two different sensors.
5. Stop the simulation.
6. Add a complementary filter into the model to verify the sensor fusion technology described in Section 6.5.
7. Run the model with a complementary filter. Compare the pitch angle signal before and after sensor fusion.

Exercise \(\PageIndex{1}\)

Show your model and results in the lab report. Figures B.80 and B.81 show the reference model and results, respectively.