Thursday, 13 February 2020

Custom Flight Controller Part 2.3.4: Sensor Fusion --- (Optional) QKF on STM32

The same quaternion kalman filter described in Part 2.3.1 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.

No comments:

Post a Comment