FPGA实现卡尔曼滤波算法——融合MPU6050的Acc和Gyro

参考GITHUB大神DOA.c作品：

原理回顾：

MPU6050传感器原理：

陀螺仪存在静态误差（积分运算导致），加速度计存在动态误差（重力加速度g），因此需要通过数据融合来消除误差，得到理想数据。Mpu6050是六轴姿态传感器，包括三轴Acc和三轴Gyro，gyro采样频率8000HZ，acc采样频率1000HZ，若使用DMP进行数据融合，angle数据的采样频率将降至200HZ。因此选用方案为：读取acc和gyro原始数据，利用卡尔曼滤波进行数据融合。

卡尔曼滤波实现步骤主要包括：

选择状态量、观测量→构建方程→初始化参数→代入公式迭代→调节超参数

下图为前两个步骤的图解：

 其中，状态量选定为角度值和陀螺仪温漂、观测值选定为角度；

方程建立为转换矩阵方程，转换过程后续补上；

根据上述方程，即可用C语言编写代码。

C语言实现:

代码实现逻辑如下：

预测当前角度→预测协方差矩阵→建立测量方程→计算卡尔曼增益→计算当前最优估计值→更新协方差矩阵

根据上图方程撰写代码；

选定初始值；

在匿名上位机调节参数

FPGA实现：

项目架构框图如下：

计算流程如下：

 接下来一步步学习一下代码逻辑：

IIC读取原始数据——在IIC_master.v中，通过iic协议，读取三轴acc数据、temp数据和三轴gyro数据，之后将数据输入kalman_filter.v。

卡尔曼滤波——分为校准和计算两部分，原始数据输入kalman_calibration.v中，进行数据校准。之后将校准后的值和原始数据一起输入kalman_calculate.v中。

在kalman_calculate.v解算三轴acc数据和三轴gyro数据，分别得到acc的roll和pitch、gyro的gyro_x和gyro_y。此处用到的运算包括：减法修正、开方、加法、平方根、补足小数位、位运算等。

接下来将这4个数据和gyro_bias一起输入Kalman_Iter_Unit中。该模块包括Kalman_Flow_Ctrl.v、Kalman_Forecast.v和Kalman_Update.v。Kalman_Flow_Ctrl.v内部有一个状态机，通过这个状态机来控制pitch和roll的预测和更新，输入的观测值为acc计算得到的pitch和roll，以及gyro_x和gyro_y。

Kalman_Forecast.v和Kalman_Update.v根据公式计算即可，基本为加减法，Kalman_Update.v有一个除法。通过flow_ctrl控制迭代即可。