Tuesday, 17 January 2017

Custom Flight Controller Part 2.8: Getting the Orientation --- Processing Orientation Visualization

The last step is to visualize the orientation to check whether the orientation calculated correspond to the real orientation. Previously, Matlab was used which proved to be slow. Therefore, processing is used this time. It has been a popular choice for orientation visualization and there are a lot of ready-to-use packages to aid the development. I have chosen to follow the steps indicated in tutorial from DIY Hacking by Arvind Sanjeez.

The serial function inside the processing code provided by the tutorial has to be modified however to interface with serial communication sent from STM32. So I have configured STM32 to send quaternion in the following format. 
For example, if the quaternion is [1 0 0 0], it would be printed as "Q: 1 0 0 0". 
Character "Q:" can be used to check whether the data received in processing is correct. To extract the quaternion from the string received. The following code is implemented.
functions like split() and trim() are inbuilt processing functions. Their documentation can be readily found on processing's website. 

I have to admit that I did not study deep into processing due to time constraint. Therefore, I am unable to explain every line of the code that are provided by the aforementioned tutorial. There might be some modification and optimization that needs to be done. Anyway, the code works. 

To run the overall simulation, 
1. Run the program on STM32 from IAR embedded workbench
2. Use a serial terminal such as Putty and follow the instructions to calibrate magnetometer and collect initial values.
3. Close Putty (Otherwise a port conflict will happen) and run the processing script to visualize orientation.

The result is shown in the video below.
The orientation visualization is much better than that previously. Yaw, pitch, roll as well as arbitrary rotation all yield pretty good results.

But it still remains to be proven that this could be implemented on to a real quadcopter. But for that, control systems have to be designed and programmed first. So this finally concludes Part 2 --- Getting the orientation. Future posts will be about Part 3 --- Control System.

A big thank you to everyone who has supported and helped me so far ~~

Sunday, 15 January 2017

Custom Flight Controller Part 2.7: Getting the Orientation --- Mag Calibration and Overall Algorithm Structure.

Magnetometer measures the direction and magnitude of magnetic field present. It is a great tool to correct yaw measurement. However, it needs calibration before it can be used to update orientation. For example, if a uncalibrated magnetometer measures 100 milliGause in x direction currently, rotating the magnetometer around z axis for 180 degrees should theoretically yield -100 milliGauss. But since the magnetometer is uncalibrated, it might give you a value like -50 millGauss. In this case, a -25 milliGauss bias should be added to the magnetometer x axis measurement so that the two measurements become +75 and -75 and are just opposite to each other.  This procedure of determining the magnetometer bias is done via magnetometer calibration. 

The method proposed in this post by Kris Winer is adopted to calibrate magnetometer in this project. Essentially, user needs to rotate the magnetometer non-stop during the calibration until the magnetomer has collected enough sample data. Then the bias of an axis is calculated as the average value of the minimum value and the maximum value measured on that axis.

The code is shown below. It is very similar to the code provided by the Kris Winer. The code is grouped under a function called calibrateMag();

After getting the magnetometer biases, these biases need to be subtracted away from each magnetometer measurement as shown below.

Other than mag calibration, there are one more task that need to be done before running orientation calculation, namely, getting the initial measurement value of magnetometer and accelerometer.

In order to calculate orientation, one need to know the sensor measurements when the copter is lying on the ground. These initial values can not be known theoretically because of the ever-changing environment so they have to be obtained experimentally. Therefore, the program will ask user to put the MPU9250 to rest after magnetometer calibration. During this period of time the program will collect 100 sets of data from MPU9250 and the average of these data is stored as the initial measurements. The code is reproduced below.

As seen above, after the initial values of the three sensors are collected and calculated, the initDataRdy flag is  set, allowing the program to run Quaternion Kalman Filter to calculate orientation.

The overall main.c structure can be summarized as follows:
Step 1. Initialize STM32 clock and peripherals
Step 2. Check MPU9250 is accessible and calibrate Magnetometer
Step 3. Start infinite loop
Step 4. (Inside loop) check whether initial data has been collected, if not, collect data. 
Step 5. (Inside loop)continue collecting data for initial data calculation until 100 sets of data has been collected.
Step 6. (Inside loop) Calculate the initial data and set initial data ready flag.
Step 7. (Inside loop) Get data from MPU9250 and calculate device orientation using QKF.

The resultant quaternion, indicating the current attitude is published via serial terminal to my laptop. The next post will explain how to use processing to read the published data and visualize the attitude of MPU9250. 

Thursday, 12 January 2017

Custom Flight Controller Part 2.6: Getting the Orientation --- Using Interrupt to Get Data from MPU9250

In previous method described in post Part 2.1, Arduino tries to get the measurement data from MPU9250 without checking whether a new data is available. Since Arduino is relatively slow, this method does not cause any problem as a new set of measurement is usually available before one iteration of the loop is completed. However, since STM32 is much faster, using interrupt is deemed to be a better choice. 

Therefore, with the new method, MPU9250 is programmed to send an interrupt to a GPIO pin of the STM32 microcontroller. When the microcontroller detects the interrupt, a data ready flag is set. The flag is checked before the execution of a new iteration. If the flag is set, STM32 will then fetch data from MPU9250 and perform sensor fusion. If the flag is not set, STM32 will start a new iteration. This posts will briefly explain the procedure to implement the method proposed above.

First, the interrupt needs to be enabled in MPU9250. This is done by configuring the Interrupt Enablee Register (Register 56) of the MPU9250. By setting the last bit of this register to 1, the data ready interrupt is propagated to interrupt pin. Furthermore, Register Int Pin/Bypass Enable Configuration (Register 55) needs to be configured to set the property of the data ready interrupt. The description of the Register is summarized in figure below. This is from the Register Map of MPU9250 provided by Invensense.
My choice for the property of interrupt is highlighted in the figure above. Therefore, I have to set the value of this register to 0x00100010 (0x22). Bit[1] is set to 1 to allow me to access magnetometer as described in Part2.1. Bit[0] is reserved so it should not be changed.The original value of Bit[0] is 0. Setting of the register is done using the following code. 
Code commented out at the bottom is the code used to send data via I2C in Arduino. Intuitively, since now the platform is STM32, corresponding STM32 peripheral library needs to be used instead. ST has provided the library named HAL. The initialization of HAL peripheral library is done by the auto code generation of STM32CubeMX so it can be used readily. So here the function HAL_I2C_Mem_Write is used to send the data to MPU9250 via I2C. 

I have migrated the whole MPU9250 library I used before from Arduino to STM32 by rewriting all the I2C functions. To save time and work, majority of the original library code was deleted. Only those necessary functions that this project will use are saved. The migrated library can be found here and here.

The next step is to configure the GPIO pin of STM32 to serve as an interrupt detector. This is done again via STM32CubeMX.
As shown in the figure above, pin PF12 is configured as GPIO_EXTI12 which means PF12 is used to detect external interrupt. PF12 is chosen arbitrarily and it maps to the PIN named D8 on Nucleo. The next step is to navigate to configuration panel, select GPIO and configure the pin as shown in the figure below.

 Then select NVIC and enable EXT1 line interrupt by checking it as shown in the figure below. 
Then code generation of STM32CubeMX will automatically configure and enable the interrupt. However, user is still required to write the custom interrupt handler.

Interrupt handler is written in a file called stm32f4xx.c not main,c. The file is by default located in USER folder in IAR workspace.  Inside this file there is a function called EXTI15_10_IRQHandler and this function is the interrupt handler of interrupt that happen on pin PF12. So when an interrupt happens on PF12, the MPU9250 data ready flag should be set to 1. This done by the following code. 

Note that the variable MPU9250_DataRdyFlag is defined in main.c. Therefore, it should be declared "extern" in stm32f4xx.c as shown below so the compiler knows that this variable is the same variable used in main.c 
The final step is to define data ready flag in main.c and check whether that is set at the start of every iteration of the main loop. This is demonstrated in the following figure. 
So that concludes the steps needed to get data from MPU9250 by interrupt. The next post will attempt to explain the structure of the main loop. 

Wednesday, 11 January 2017

Custom Flight Controller Part 2.5: Getting the Orientation --- QKF on STM32

The same quaternion kalman filter described in Part 2.2 was re-written in C in IAR embedded workbench to be used on STM32 microcontroller. This posts will briefly introduce and explain the migrated code. 

Eigen, a popular c++ library for linear algebra unfortunately can not be recognized and compiled by the IAR C++ compiler. In the end, I chose CMSIS DSP Software Library, a library developed specifically for ARM Cortex-M processors, to perform matrix arithmetic. Correspondingly, “arm_math.h” has to be included in main.c. (arm_math.h is auto generated and recognized, you don't have to manually download or install it)

The basic format of initializing a matrix using CMSIS DSP Software Library looks like follows. 

After the initialization, the matrix named matrix1 is bounded to the float array matrix1_data. Any change to the matrix instance matrix1 will change the value of float array matrix1_data and vice versa.  

The library has provided all the necessary functions for basic matrix arithmetic including addition, multiplicationmatrix inverse and matrix transpose. The matlab code was translated to C step by step utilizing these functions. 

The complete code can be found in my Github. Look for the function runKalmanFilter().
The original quaternion kalman filter written in Matlab can be found here or refer to Part 2.2.

For example, the code below translate the matlab statement commented out on top to C code. It is quite straight-forward. 
Unfortunately, C does not allow construction of matrix as easily as Matlab. Therefore, for matrix construction, I used the old stupid way---manually copying values into the float array. An example is shown below. Again, the comment at top shows the original Matlab code. 
An interesting problem occurred after I finished translating the whole algorithm. With the same input dummy data, the resultant quaternion calculated by STM32 is found to be different from that calculated by Matlab. After much investigation, I found out that the calculation result of floating point arithmetic by STM32 is wrong at values which are supposed to be 0. Instead, it gives me answer that is of the order 1e-8. Although it is fairly close to 0, but unfortunately, it is not acceptable in this case as other non-zero values are also of the same order. For example, as shown in the above figure, during calculation of matrix A, a coefficient dt*dt/4 is multiplied. Dt is the time between each sensor measurement. Since the sensor is measuring data at 1000hz, dt = 0.001. Therefore, matrix A's values are all extremely small. 

The reason behind this calculation error I believe is because IEEE 754 floating point can not actually represent some numbers exactly. For example 0.1 can not be represented exactly. I do not know any good method to solve this problem so I have adopted a very crude way.
I assume that value zero is only supposed to occur at first iteration of the kalman filter as matrix P and X are populated after the first iteration, that is they no longer contains 0. (Matrix P is the process noise covariance matrix, it is initialized as identity matrix while Matrix X is the state matrix, it is initialized as a unit quaternion [1 0 0 0]). With that assumption, I have added a flag indicating the first iteration. If that flag is true, corresponding values are manually set to 0. An example is shown in the figure below.

I am not sure whether this method actually solved the problem. Fortunately, the test result looks promising. If reader has an idea of how to solve this problem, please let me know by leaving a comment. 

That is all for this post, the next post will explain how to get data from MPU9250 sensor module via interrupt.

Tuesday, 10 January 2017

Custom Flight Controller Part 2.4: Getting the Orientation --- Migrate to STM32

As mentioned in the previous posts, the Matlab simulation has several issues and can only track slow rotations. Therefore, during the December holiday, I have managed to migrate the whole algorithm to a proper microcontroller of STM32F4 series and obtained much better simulation result from there.

With the new method, the microcontroller gets the data from MPU9250 using interrupt, calculates the orientation and publishes the resultant quaternion to my laptop via USB serial communication. Processing was then used to collect the quaternion data and visualize the result for testing. As the calculation of Quaternion Kalman Filter is now done by MCU instead of by Matlab, it solved the problem in previous posts.

The development board I bought was NucleoF429ZI. As the name suggests, the microcontroller it equips is STM32F429 which runs at 180MHZ which should be more than enough for our flight controller simulation. It also features a 2M Flash and includes all the necessary peripherals that you could find on an Arduino.

The corresponding IDE I chose was IAR embedded workbench.Another useful tool I used was STM32CubeMX provided by ST. It provides a simple GUI tool for you to enable corresponding peripherals and auto generates initialization code. 
For this project, I need to enable I2C peripheral to interface with my MPU9250 sensor module from SeeedStudio as well as UART for serial communication with my laptop. The picture below shows the corresponding configuration inside CubeMX.
On the left side of the image, both I2C1 and USART3 are labelled green, showing that they are enabled. After which you can click project->generated code to generate initialization code for STM32. EWARM is selected as toolchain as I am using IAR embedded workbench as IDE. More detailed tutorials of how to use CubeMX can be found here
Completing code generating will give you a folder that look like this.

An extremely useful feature of using STM32CubeMX is that you don't have to configure anything, all the necessary drivers and files are already being grouped nicely into individual folders. Opening
 EWARM->project.eww led me straight into IAR workspace as shown in the figure below.
main.c has already included the initialization code for I2C and UART as well as other necessary system initialization. The next step is to change the project language from c by default to C++ as I am writing OOP. It could be easily changed by right-clicking project name, select options->c/c++  compiler and select c++ under language 1 as shown in the figure below.
The next step is re-write all the code that I used previously into IAR workspace. My complete code could be found in my Github. Future posts will cover the code in more detail as well. 

Wednesday, 16 November 2016

Custom Flight Controller Part 2.3: Getting the Orientation --- The Matlab Simulation

This post records down the steps that I did to achieve the final matlab simulation as well as discusses limitations of this method and suggests future improvements.

From previous steps I was able to obtain the data from IMU sensors and implemented a quaternion Kalman Filter for orientation calculation in matlab. The final step is therefore to use matlab to perform orientation visualization so as to prove that this the proposed algorithm can successfully keep track of the device's orientation.

Matlab provides existing functions for serial communication. The official tutorial could be found here. Again,my script could be found in my Github. In this post, I will try to explain the essential parts of my code.

This screenshot below is from serial_func.m and it demonstrates how to initialize a serial communication in matlab and configure its relevant parameters.
Note that I have added in a callback function for serial communication. The last two lines of the code shown above essentially tells the program to execute the function named callback whenever a new line is detected.
Remember that the data output by arduino is of the following format as described in my previous post.  

Therefore, I implemented the following functions in callback.m to extract the data. 

The elaspedTime is measured in microseconds, therefore it has to be translated to seconds for calculating the rotation angles. The reason to add another 10 millisecond on top of the measurement is that I later found out the matlab simulation is not fast enough. My matlab cannot complete executing the filter and updating the simulation graph before the next data arrives. Therefore, I have to arbitrary decrease the data publication frequency of arduino by adding delay(10) at the end of the loop() in Arduino. Therefore, the elapsedTime, aka the time for which a single gyro data is valid is arbitrarily lengthened by 10 millisecond.

Note that it is not desirable as it exacerbates the problem described in my previous post, "getting data from MPU9250", that this method could only track orientation correctly if the IMU is rotated slowly. The way to fix it is to use another method of simualation, most likely Processing. For that, I have to migrate the filter from matlab to microcontroller. Since arduino does not have a floating point unit, I plan to purchase STM32 microcontroller. I will publish further posts in the future. 
Nonetheless, as long as the IMU is not rotated fast, this method is sufficient to demonstrate that the quaternion Kalman Filter works. 

Another additional function that I have added in to my filter was to check whether the IMU was experiencing external linear acceleration. External linear acceleration on top of the gravity will contaminate the accelerometer measurement and therefore the orientation updated. There are several methods to overcome this problem. I chose the easiest and most straight-forward, threshold switching. If the magnitude of accelerometer measurement exceeds the gravity by a certain threshold, it is interpreted as that external linear acceleration is present. Under this situation, accelerometer measurements will not be used to update the prediction given by gyroscope. This is achieved by setting the Kalman Gain of the accelerometer measurement to be zero matrix as shown below:

After getting the data, simply execute the filter by passing all relevant measurement into quaternion kalman filter that has been developed previously along with the extAcc flag. 
The result given by the quaternion kalman filter would be the updated state, that is a quaternion representing the current orientation. Executing quat2eul() function in matlab will generate a rotation matrix based on the given quaternion. My method of simulating the orientation in matlab is to first draw a horizontal rectangle in matlab figure, then rotate the rectangle by premultiplying its coordinates with the rotation matrix generated. The result rectangle shown in the graph therefore should represent the final orientation of the IMU device, again, as long as the IMU is not rotated too fast.

Note that at about second, error occurred because I rotated the device too fast. Nonetheless, the simulation shows that the implemented algorithm can track the orientation relatively accurately.

However, the results demonstrated is far from being satisfactory. The current performance of the filter can not be implemented on quadcopter. Therefore, I will migrate the whole algorithm to STM32 Microcontroller. The Quoternion Kalman Filter will be re-written in C++ and run by the microcontroller directly instead of Matlab. Processing will be used to do do the same orientation visualization. Remember the most prominent problem faced here is that gyroscope data is assumed to be valid far longer than its real value. With the microcontroller having real-time operation system, this problem could be solved. I expect a better result then.

Nonetheless, I think it is safe to draw the conclusion that there is no problem in the implementation of Quaternion Kalman Filter. Since I am focusing on the software part of the custom flight controller first, I will not proceed to buy the STM32 microcontroller and write the code there immediately. Instead, I plan to focus on the flight control algorithm in this upcoming December.

I hope that I can post some fruitful results soon.

Wednesday, 9 November 2016

Custom Flight Controller Part 2.2: Getting the Orientation --- The Quaternion Kalman Filter

Accelerometer measures the acceleration experienced, gyroscope measures the angular velocity and magnetometer measures the magnetic field. In order to calculate the orientation from these IMU measurements, sensor fusion needs to be performed. Although there are numerous algorithm to achieve sensor fusion, the underlying principle should be similar.
Image result for IMU sensor fusion
An example of sensor fusion using complementary filter
Assuming the initial orientation is known, integrating the gyroscope measurement will indicate how much the device has rotated. Combining both information allows me to predict the current orientation. Furthermore, assuming that there is no external linear acceleration, the only acceleration the accelerometer should measure is gravity which is always pointing downward. With different orientation of the device gravity measured will give different xyz axis measurement while the magnitude is remained constant. Therefore, accelerometer measurement could be used to update the orientation predicted to give a better estimate. The same theory can be applied to magnetometer as well.

However, sensor measurements are not accurate. Not only does every sensor has intrinsic noise, there are numerous problems that affect the accuracy of measurement. To name a few, gyroscope tends to drift over time, meaning it will have a zero-state error. And external acceleration and magnetic field are always present in real life so the accelerometer and magnetometer measurement under such conditions would be contaminated. The purpose of sensor fusion is therefore to make us of all the available data and give the best possible estimate of the device's current orientation.

Popular sensor fusion methods include complimentary filter which is widely used in self-balancing robot, extended Kalman Filter which is used in Pixhawk. Similarly, there are countless papers explaining various types of filters to perform orientation estimation. I have decided to use a quaternion kalman filter for this project. This filter algorithm is proposed and explained in paper Novel Quaternion Kalman Filter and Adaptive Kalman Filter for MEMS-IMU based Attitude Estimation under External Acceleration and Parsimonious use of Gyroscopes.

In order to understand the filter algorithm, one has to understand the basics of Kalman Filter first. This amazing tutorial on Extended Kalman Filter by Simon D. Levy benefited me greatly. And if you are looking for mathematical proof, Wikipedia offers it. Alternatively, here contains a complete derivation of Extended Kalman Filter.

Since I was using a Quaternion Kalman Filter, I had to get myself familiar with quaternion as well. Wikipedia was my main teacher in this case.  

I do not have the confidence to say that I have completely understood the mathematical theories behind Kalman Filter and quaternion, but these tutorials have enabled me to implement the filter proposed in research paper. The implementation of the algorithm is explained as follows.  
  • Let X be the state quaternion representing the orientation. X = [x0 x1 x2 x3] = [x0 x*], where x0 is the scalar and x* = [x1 x2 x3] forms the vector part of quaternion. 
  • Let A be the state transition matrix. Quaternion algebra gives that 
where w =[gyroX gyroY gyroZ] is a vector that contains the gyroscope measurement. The proof of this equation can be found here. The weird operator in the middle represents quaternion multiplcation. Quaternion multiplication could be changed to matrix multiplication as shown in this figure


  • Let Q be the process noise covariance matrix.Then 
where Rg is the gyroscope measurement noise covariance. Its diagonal elements indicate the variance of the measurement data in three axes. And
Basically, this formula relates the gyroscope measurement noise covariance to the noise of quaternion predicted. It is somewhat like a frame transformation. Similarly, let R be the observation noise convariance matrix. Then
where Ra is the accelerometer measurement noise covariance matrix.
  • The acceleration observation model is 

Where Z is the accelerometer measurement, f(Xk) is a function of Xk, G is the gravitation acceleration vector and V represents noise. Normally, Extended Kalman Filter is used to handle this kind of nonlinear observation model. However, here the papers proposed a different method. Left-multiplying both sides of the above equation by Xk and rearranging yields
Note that the left side of the equation becomes zero. And the observation model is linearized, allowing us to use a linear Kalman filter without having to compute Jacobians as required in Extended Kalman Filter. Now the measurement residual(the difference between calculated value and measurement) is -HXk.
  • The rest are quite straight forward. Following the Kalman Filter Algorithm we get:

Since Arduino is not capable to handle matrix math and floating point calculation fast enough, I implement the full algorithm in matlab. The test script can be found in my Github.
Inside the script I have arbitrary set gyro measurement to some random values. I have also set dt to one for simplicity. 

gx_true = 20;
gy_true = 45;
gz_true = 90;
dt = 1;

Noises are added by utilizing random number generator. Note that Kalman Filter assumes noise to be normally distributed bu RNG gives uniform distribution. However, it does not affect much.

%add process noise
gx = gx_true+ 10*rand(1,1) + (-5);
gy = gy_true+ 10*rand(1,1) + (-5);
gz = gz_true+ 10*rand(1,1) + (-5);

%add measurement noise
tmp = 4*rand(3,1)+(-2);
Z = D_true*G +tmp;

D_true is the rotation matrix generated from quaternion. Matlab provides an existing function quat2eul() to calculate this rotation matrix.

Running the qkf.m gives the following result

The yellow dotted line is the true value, orange dashed line represent measurements and the blue curve represents the estimated value. This shows that the proposed quaternion kalman filter is able to reject noise and maintain a relatively good estimate.

That is all for the quaternion kalman filter. The next part will be about running the actual simulation on matlab.