基于STM32F103ZET6的MPU6050卡尔曼滤波角度测量系统
一、系统架构设计
1.1 硬件配置
主控芯片：STM32F103ZET6（72MHz，512KB Flash，64KB RAM）
姿态传感器：MPU6050（6轴：3轴加速度计 + 3轴陀螺仪）
通信接口：I2C1（PB6-SCL，PB7-SDA）
显示模块：0.96寸OLED（I2C2，PB10-SCL，PB11-SDA）
调试接口：USART1（PA9-TX，PA10-RX）
电源管理：3.3V LDO，100nF去耦电容
1.2 卡尔曼滤波原理
状态方程：X(k) = A·X(k-1) + B·U(k) + W(k)
观测方程：Z(k) = H·X(k) + V(k)
状态向量：X = [角度, 角速度]ᵀ
控制输入：U = 陀螺仪测量值
观测值：Z = 加速度计计算的角度
卡尔曼增益：K(k) = P(k|k-1)·Hᵀ·(H·P(k|k-1)·Hᵀ + R)⁻¹
状态更新：X(k|k) = X(k|k-1) + K(k)·(Z(k) - H·X(k|k-1))
协方差更新：P(k|k) = (I - K(k)·H)·P(k|k-1)
二、完整工程源码
2.1 主程序（main.c）
/**
* @file main.c
* @brief STM32F103ZET6 + MPU6050 卡尔曼滤波角度测量
* @version 2.0
*/
#include "stm32f10x.h"
#include "mpu6050.h"
#include "kalman.h"
#include "oled.h"
#include "uart.h"
#include "delay.h"
#include "math.h"
/* 系统参数 */
#define SAMPLE_RATE_HZ 200 // 采样率200Hz
#define DT 0.005f // 采样周期5ms (1/200Hz)
#define RAD_TO_DEG 57.295779513f // 弧度转角度
/* 全局变量 */
KalmanFilter kalman_roll; // 横滚角卡尔曼滤波器
KalmanFilter kalman_pitch; // 俯仰角卡尔曼滤波器
MPU6050_Data mpu_data;
AngleData angle_data;
SystemStatus sys_status;
/* 角度数据结构 */
typedef struct {
float roll; // 横滚角（绕X轴）
float pitch; // 俯仰角（绕Y轴）
float yaw; // 航向角（绕Z轴）
float roll_rate; // 横滚角速度
float pitch_rate; // 俯仰角速度
float yaw_rate; // 航向角速度
uint8_t is_valid; // 数据有效性
} AngleData;
/* 系统状态 */
typedef struct {
uint32_t sample_count; // 采样计数
uint32_t error_count; // 错误计数
float cpu_usage; // CPU使用率
uint8_t mpu_connected; // MPU6050连接状态
} SystemStatus;
int main(void)
{
/* 1. 系统初始化 */
System_Init();
/* 2. 外设初始化 */
Delay_Init();
OLED_Init();
MPU6050_Init();
UART_Init(115200);
Kalman_Init(&kalman_roll);
Kalman_Init(&kalman_pitch);
/* 3. 检查MPU6050连接 */
if (MPU6050_TestConnection())
{
sys_status.mpu_connected = 1;
OLED_ShowString(0, 0, "MPU6050 Connected");
UART_SendString("MPU6050 Connected!\r\n");
}
else
{
sys_status.mpu_connected = 0;
OLED_ShowString(0, 0, "MPU6050 Error!");
UART_SendString("MPU6050 Connection Failed!\r\n");
while(1);
}
OLED_Refresh();
Delay_Ms(1000);
/* 4. 主循环 */
while(1)
{
/* 4.1 读取MPU6050数据 */
if (MPU6050_ReadData(&mpu_data))
{
/* 4.2 计算加速度计角度（静态） */
float accel_roll = atan2(mpu_data.ay, mpu_data.az) * RAD_TO_DEG;
float accel_pitch = atan2(-mpu_data.ax, sqrt(mpu_data.ay*mpu_data.ay + mpu_data.az*mpu_data.az)) * RAD_TO_DEG;
/* 4.3 陀螺仪数据转换（转为度/秒） */
float gyro_roll_rate = mpu_data.gx * 250.0f / 32768.0f; // ±250°/s量程
float gyro_pitch_rate = mpu_data.gy * 250.0f / 32768.0f;
float gyro_yaw_rate = mpu_data.gz * 250.0f / 32768.0f;
/* 4.4 卡尔曼滤波 */
float kalman_roll_out = Kalman_Update(&kalman_roll, accel_roll, gyro_roll_rate, DT);
float kalman_pitch_out = Kalman_Update(&kalman_pitch, accel_pitch, gyro_pitch_rate, DT);
/* 4.5 更新角度数据 */
angle_data.roll = kalman_roll_out;
angle_data.pitch = kalman_pitch_out;
angle_data.yaw += gyro_yaw_rate * DT; // 航向角直接积分
angle_data.roll_rate = gyro_roll_rate;
angle_data.pitch_rate = gyro_pitch_rate;
angle_data.yaw_rate = gyro_yaw_rate;
angle_data.is_valid = 1;
/* 4.6 显示数据 */
OLED_ShowAngle(angle_data);
/* 4.7 串口输出 */
if (sys_status.sample_count % 20 == 0) // 每20次采样输出一次（10Hz）
{
UART_SendAngleData(angle_data);
}
sys_status.sample_count++;
}
else
{
sys_status.error_count++;
OLED_ShowString(0, 6, "Read Error!");
}
/* 4.8 延时控制采样率 */
Delay_Ms(5); // 200Hz采样率
}
}
/* 系统初始化 */
void System_Init(void)
{
/* 系统时钟配置：72MHz */
SystemClock_Init();
/* 中断优先级分组 */
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
/* 初始化全局变量 */
memset(&angle_data, 0, sizeof(AngleData));
memset(&sys_status, 0, sizeof(SystemStatus));
sys_status.sample_count = 0;
sys_status.error_count = 0;
sys_status.mpu_connected = 0;
}
2.2 MPU6050驱动（mpu6050.c）
/**
* @file mpu6050.c
* @brief MPU6050驱动（I2C接口）
*/
#include "mpu6050.h"
#include "i2c.h"
#include "delay.h"
/* MPU6050寄存器定义 */
#define MPU6050_ADDR 0x68 // I2C地址（AD0=0）
#define WHO_AM_I 0x75 // 器件ID寄存器
#define PWR_MGMT_1 0x6B // 电源管理寄存器1
#define SMPLRT_DIV 0x19 // 采样率分频寄存器
#define CONFIG 0x1A // 配置寄存器
#define GYRO_CONFIG 0x1B // 陀螺仪配置寄存器
#define ACCEL_CONFIG 0x1C // 加速度计配置寄存器
#define FIFO_EN 0x23 // FIFO使能寄存器
#define INT_ENABLE 0x38 // 中断使能寄存器
#define INT_STATUS 0x3A // 中断状态寄存器
#define ACCEL_XOUT_H 0x3B // 加速度计X轴高字节
#define GYRO_XOUT_H 0x43 // 陀螺仪X轴高字节
/* 初始化MPU6050 */
uint8_t MPU6050_Init(void)
{
uint8_t res;
/* 1. 初始化I2C */
I2C_Init();
/* 2. 唤醒MPU6050 */
MPU6050_WriteReg(PWR_MGMT_1, 0x00); // 解除睡眠模式
Delay_Ms(100);
/* 3. 设置采样率分频 */
MPU6050_WriteReg(SMPLRT_DIV, 0x07); // 采样率 = 陀螺仪输出率/(1+SMPLRT_DIV) = 1kHz/(1+7)=125Hz
/* 4. 配置低通滤波器 */
MPU6050_WriteReg(CONFIG, 0x06); // 5Hz低通滤波
/* 5. 配置陀螺仪量程 ±250°/s */
MPU6050_WriteReg(GYRO_CONFIG, 0x00);
/* 6. 配置加速度计量程 ±2g */
MPU6050_WriteReg(ACCEL_CONFIG, 0x00);
/* 7. 关闭FIFO */
MPU6050_WriteReg(FIFO_EN, 0x00);
/* 8. 关闭中断 */
MPU6050_WriteReg(INT_ENABLE, 0x00);
/* 9. 验证器件ID */
res = MPU6050_ReadReg(WHO_AM_I);
if (res != 0x68)
{
return 0; // 初始化失败
}
return 1; // 初始化成功
}
/* 测试MPU6050连接 */
uint8_t MPU6050_TestConnection(void)
{
uint8_t who_am_i = MPU6050_ReadReg(WHO_AM_I);
return (who_am_i == 0x68) ? 1 : 0;
}
/* 读取MPU6050数据 */
uint8_t MPU6050_ReadData(MPU6050_Data* data)
{
uint8_t buf[14];
/* 读取14个寄存器（加速度计+陀螺仪） */
if (MPU6050_ReadRegs(ACCEL_XOUT_H, buf, 14))
{
/* 解析加速度计数据（16位有符号） */
data->ax = (int16_t)((buf[0] << 8) | buf[1]);
data->ay = (int16_t)((buf[2] << 8) | buf[3]);
data->az = (int16_t)((buf[4] << 8) | buf[5]);
/* 跳过温度数据（2字节） */
/* 解析陀螺仪数据（16位有符号） */
data->gx = (int16_t)((buf[8] << 8) | buf[9]);
data->gy = (int16_t)((buf[10] << 8) | buf[11]);
data->gz = (int16_t)((buf[12] << 8) | buf[13]);
return 1;
}
return 0;
}
/* 写寄存器 */
void MPU6050_WriteReg(uint8_t reg, uint8_t data)
{
I2C_Start();
I2C_SendByte(MPU6050_ADDR << 1); // 写地址
I2C_WaitAck();
I2C_SendByte(reg);
I2C_WaitAck();
I2C_SendByte(data);
I2C_WaitAck();
I2C_Stop();
}
/* 读寄存器 */
uint8_t MPU6050_ReadReg(uint8_t reg)
{
uint8_t data;
I2C_Start();
I2C_SendByte(MPU6050_ADDR << 1); // 写地址
I2C_WaitAck();
I2C_SendByte(reg);
I2C_WaitAck();
I2C_Start();
I2C_SendByte((MPU6050_ADDR << 1) | 0x01); // 读地址
I2C_WaitAck();
data = I2C_ReadByte(0); // 发送NACK
I2C_Stop();
return data;
}
/* 连续读多个寄存器 */
uint8_t MPU6050_ReadRegs(uint8_t reg, uint8_t* buf, uint8_t len)
{
I2C_Start();
I2C_SendByte(MPU6050_ADDR << 1); // 写地址
I2C_WaitAck();
I2C_SendByte(reg);
I2C_WaitAck();
I2C_Start();
I2C_SendByte((MPU6050_ADDR << 1) | 0x01); // 读地址
I2C_WaitAck();
for (uint8_t i = 0; i < len; i++)
{
if (i == len - 1)
buf[i] = I2C_ReadByte(0); // 最后一个字节发送NACK
else
buf[i] = I2C_ReadByte(1); // 发送ACK继续读
}
I2C_Stop();
return 1;
}
/* 设置陀螺仪偏移校准 */
void MPU6050_Calibrate(void)
{
int32_t gyro_x_offset = 0, gyro_y_offset = 0, gyro_z_offset = 0;
MPU6050_Data data;
/* 采集100次数据求平均 */
for (int i = 0; i < 100; i++)
{
MPU6050_ReadData(&data);
gyro_x_offset += data.gx;
gyro_y_offset += data.gy;
gyro_z_offset += data.gz;
Delay_Ms(10);
}
/* 计算偏移量 */
gyro_x_offset /= 100;
gyro_y_offset /= 100;
gyro_z_offset /= 100;
/* 保存偏移量（在实际应用中应保存到EEPROM） */
// 后续使用时减去偏移量
}
2.3 卡尔曼滤波算法（kalman.c）
/**
* @file kalman.c
* @brief 卡尔曼滤波算法实现（针对角度测量）
*/
#include "kalman.h"
#include "math.h"
/* 初始化卡尔曼滤波器 */
void Kalman_Init(KalmanFilter* kf)
{
/* 状态向量初始化 */
kf->x[0] = 0.0f; // 角度
kf->x[1] = 0.0f; // 角速度
/* 状态协方差矩阵初始化 */
kf->P[0][0] = 1.0f; kf->P[0][1] = 0.0f;
kf->P[1][0] = 0.0f; kf->P[1][1] = 1.0f;
/* 过程噪声协方差矩阵Q */
kf->Q[0][0] = 0.001f; kf->Q[0][1] = 0.0f;
kf->Q[1][0] = 0.0f; kf->Q[1][1] = 0.003f;
/* 观测噪声协方差R */
kf->R = 0.5f; // 加速度计测量噪声
/* 状态转移矩阵A */
kf->A[0][0] = 1.0f; kf->A[0][1] = 1.0f; // 角度 = 上一时刻角度 + 角速度*dt
kf->A[1][0] = 0.0f; kf->A[1][1] = 1.0f; // 角速度保持不变
/* 观测矩阵H */
kf->H[0] = 1.0f; // 观测角度
kf->H[1] = 0.0f; // 不观测角速度
/* 控制输入矩阵B */
kf->B[0] = 1.0f; // 角度受角速度影响
kf->B[1] = 0.0f; // 角速度不受控制输入影响
}
/* 卡尔曼滤波更新 */
float Kalman_Update(KalmanFilter* kf, float z_measure, float u_control, float dt)
{
/* 1. 预测步骤 */
float x_pred[2]; // 预测状态
float P_pred[2][2]; // 预测协方差
/* 更新状态转移矩阵（考虑dt） */
kf->A[0][1] = dt; // 角度 = 角度 + 角速度*dt
/* 状态预测：x_pred = A * x + B * u */
x_pred[0] = kf->A[0][0] * kf->x[0] + kf->A[0][1] * kf->x[1] + kf->B[0] * u_control;
x_pred[1] = kf->A[1][0] * kf->x[0] + kf->A[1][1] * kf->x[1] + kf->B[1] * u_control;
/* 协方差预测：P_pred = A * P * A^T + Q */
// 简化计算：P_pred = A * P * A^T + Q
P_pred[0][0] = kf->A[0][0]*kf->P[0][0]*kf->A[0][0] + kf->A[0][1]*kf->P[1][0]*kf->A[0][0] +
kf->A[0][0]*kf->P[0][1]*kf->A[0][1] + kf->A[0][1]*kf->P[1][1]*kf->A[0][1] + kf->Q[0][0];
P_pred[0][1] = kf->A[0][0]*kf->P[0][0]*kf->A[1][0] + kf->A[0][1]*kf->P[1][0]*kf->A[1][0] +
kf->A[0][0]*kf->P[0][1]*kf->A[1][1] + kf->A[0][1]*kf->P[1][1]*kf->A[1][1] + kf->Q[0][1];
P_pred[1][0] = kf->A[1][0]*kf->P[0][0]*kf->A[0][0] + kf->A[1][1]*kf->P[1][0]*kf->A[0][0] +
kf->A[1][0]*kf->P[0][1]*kf->A[0][1] + kf->A[1][1]*kf->P[1][1]*kf->A[0][1] + kf->Q[1][0];
P_pred[1][1] = kf->A[1][0]*kf->P[0][0]*kf->A[1][0] + kf->A[1][1]*kf->P[1][0]*kf->A[1][0] +
kf->A[1][0]*kf->P[0][1]*kf->A[1][1] + kf->A[1][1]*kf->P[1][1]*kf->A[1][1] + kf->Q[1][1];
/* 2. 更新步骤 */
/* 计算卡尔曼增益：K = P_pred * H^T * (H * P_pred * H^T + R)^-1 */
float S = kf->H[0] * P_pred[0][0] * kf->H[0] + kf->H[1] * P_pred[1][0] * kf->H[0] + kf->R;
float K[2];
K[0] = (P_pred[0][0] * kf->H[0] + P_pred[0][1] * kf->H[1]) / S;
K[1] = (P_pred[1][0] * kf->H[0] + P_pred[1][1] * kf->H[1]) / S;
/* 更新状态：x = x_pred + K * (z - H * x_pred) */
float y = z_measure - (kf->H[0] * x_pred[0] + kf->H[1] * x_pred[1]); // 观测残差
kf->x[0] = x_pred[0] + K[0] * y;
kf->x[1] = x_pred[1] + K[1] * y;
/* 更新协方差：P = (I - K * H) * P_pred */
float I_KH[2][2];
I_KH[0][0] = 1.0f - K[0] * kf->H[0];
I_KH[0][1] = -K[0] * kf->H[1];
I_KH[1][0] = -K[1] * kf->H[0];
I_KH[1][1] = 1.0f - K[1] * kf->H[1];
float P_new[2][2];
P_new[0][0] = I_KH[0][0]*P_pred[0][0] + I_KH[0][1]*P_pred[1][0];
P_new[0][1] = I_KH[0][0]*P_pred[0][1] + I_KH[0][1]*P_pred[1][1];
P_new[1][0] = I_KH[1][0]*P_pred[0][0] + I_KH[1][1]*P_pred[1][0];
P_new[1][1] = I_KH[1][0]*P_pred[0][1] + I_KH[1][1]*P_pred[1][1];
kf->P[0][0] = P_new[0][0]; kf->P[0][1] = P_new[0][1];
kf->P[1][0] = P_new[1][0]; kf->P[1][1] = P_new[1][1];
return kf->x[0]; // 返回滤波后的角度
}
/* 简化版一维卡尔曼滤波（仅角度） */
float Kalman_1D_Update(float* x_est, float* p_est, float z_meas, float q, float r, float u, float dt)
{
/* 预测 */
float x_pred = *x_est + u * dt;
float p_pred = *p_est + q;
/* 更新 */
float k = p_pred / (p_pred + r);
*x_est = x_pred + k * (z_meas - x_pred);
*p_est = (1 - k) * p_pred;
return *x_est;
}
2.4 I2C驱动（i2c.c）
/**
* @file i2c.c
* @brief I2C软件模拟驱动
*/
#include "i2c.h"
#include "delay.h"
/* I2C引脚定义 */
#define I2C_SCL_PIN GPIO_Pin_6
#define I2C_SCL_PORT GPIOB
#define I2C_SDA_PIN GPIO_Pin_7
#define I2C_SDA_PORT GPIOB
/* 初始化I2C */
void I2C_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
/* 使能时钟 */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
/* 配置SCL和SDA为开漏输出 */
GPIO_InitStructure.GPIO_Pin = I2C_SCL_PIN | I2C_SDA_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD; // 开漏输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
/* 拉高总线 */
I2C_SCL_H();
I2C_SDA_H();
}
/* 起始信号 */
void I2C_Start(void)
{
I2C_SDA_H();
I2C_SCL_H();
Delay_Us(5);
I2C_SDA_L();
Delay_Us(5);
I2C_SCL_L();
}
/* 停止信号 */
void I2C_Stop(void)
{
I2C_SCL_L();
I2C_SDA_L();
Delay_Us(5);
I2C_SCL_H();
Delay_Us(5);
I2C_SDA_H();
Delay_Us(5);
}
/* 等待应答 */
uint8_t I2C_WaitAck(void)
{
uint8_t ack;
I2C_SDA_H(); // 释放SDA
Delay_Us(5);
I2C_SCL_H();
Delay_Us(5);
ack = GPIO_ReadInputDataBit(I2C_SDA_PORT, I2C_SDA_PIN);
I2C_SCL_L();
Delay_Us(5);
return ack; // 0=ACK, 1=NACK
}
/* 发送字节 */
void I2C_SendByte(uint8_t data)
{
for (uint8_t i = 0; i < 8; i++)
{
I2C_SCL_L();
Delay_Us(5);
if (data & 0x80)
I2C_SDA_H();
else
I2C_SDA_L();
data <<= 1;
Delay_Us(5);
I2C_SCL_H();
Delay_Us(5);
}
I2C_SCL_L();
I2C_WaitAck(); // 等待从设备应答
}
/* 接收字节 */
uint8_t I2C_ReadByte(uint8_t ack)
{
uint8_t data = 0;
I2C_SDA_H(); // 释放SDA，准备接收
for (uint8_t i = 0; i < 8; i++)
{
I2C_SCL_L();
Delay_Us(5);
I2C_SCL_H();
Delay_Us(5);
data <<= 1;
if (GPIO_ReadInputDataBit(I2C_SDA_PORT, I2C_SDA_PIN))
data |= 0x01;
Delay_Us(5);
}
I2C_SCL_L();
/* 发送应答 */
if (ack)
I2C_SDA_L(); // ACK
else
I2C_SDA_H(); // NACK
Delay_Us(5);
I2C_SCL_H();
Delay_Us(5);
I2C_SCL_L();
return data;
}
2.5 OLED显示（oled.c）
/**
* @file oled.c
* @brief OLED显示角度数据
*/
#include "oled.h"
#include "i2c.h"
#include "font.h"
/* OLED显存 */
static uint8_t OLED_GRAM[128][8];
/* 显示角度数据 */
void OLED_ShowAngle(AngleData angle)
{
char str[20];
OLED_Clear();
/* 第一行：系统标题 */
OLED_ShowString(0, 0, "MPU6050 Kalman");
/* 第二行：横滚角 */
sprintf(str, "Roll:%6.1f", angle.roll);
OLED_ShowString(0, 2, str);
/* 第三行：俯仰角 */
sprintf(str, "Pitch:%5.1f", angle.pitch);
OLED_ShowString(0, 4, str);
/* 第四行：角速度 */
sprintf(str, "Rate:%5.1f", angle.roll_rate);
OLED_ShowString(0, 6, str);
OLED_Refresh();
}
/* 显示原始数据 */
void OLED_ShowRawData(MPU6050_Data data)
{
char str[20];
OLED_Clear();
OLED_ShowString(0, 0, "Raw Data:");
sprintf(str, "AX:%6d", data.ax);
OLED_ShowString(0, 2, str);
sprintf(str, "AY:%6d", data.ay);
OLED_ShowString(0, 4, str);
sprintf(str, "AZ:%6d", data.az);
OLED_ShowString(0, 6, str);
OLED_Refresh();
}
2.6 串口输出（uart.c）
/**
* @file uart.c
* @brief 串口输出角度数据
*/
#include "uart.h"
#include "stdio.h"
/* 发送角度数据 */
void UART_SendAngleData(AngleData angle)
{
char buffer[128];
sprintf(buffer, "Angle Data:\r\n");
UART_SendString(buffer);
sprintf(buffer, "Roll: %.2f deg, Pitch: %.2f deg, Yaw: %.2f deg\r\n",
angle.roll, angle.pitch, angle.yaw);
UART_SendString(buffer);
sprintf(buffer, "Roll Rate: %.2f deg/s, Pitch Rate: %.2f deg/s, Yaw Rate: %.2f deg/s\r\n",
angle.roll_rate, angle.pitch_rate, angle.yaw_rate);
UART_SendString(buffer);
sprintf(buffer, "----------------------------------------\r\n");
UART_SendString(buffer);
}
/* 发送CSV格式数据（用于MATLAB分析） */
void UART_SendCSVData(AngleData angle, uint32_t timestamp)
{
char buffer[128];
sprintf(buffer, "%lu,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",
timestamp,
angle.roll, angle.pitch, angle.yaw,
angle.roll_rate, angle.pitch_rate, angle.yaw_rate);
UART_SendString(buffer);
}
三、Keil工程配置
3.1 工程设置
Target:
- Device: STM32F103ZE
- Clock: 8MHz HSE → 72MHz PLL
- RAM: 64KB
- Flash: 512KB
C/C++:
- Define: USE_STDPERIPH_DRIVER, STM32F103xE
- Include Paths:
.\
..\Libraries\CMSIS\Include
..\Libraries\STM32F10x_StdPeriph_Driver\inc
..\User
..\User\drivers
..\User\algorithm
Linker:
- IROM1: 0x08000000, 0x80000 (512KB Flash)
- IRAM1: 0x20000000, 0x10000 (64KB RAM)
3.2 编译优化
Optimization Level: -O1
One ELF Section per Function: ✓
四、测试与调试
4.1 测试步骤
/* 测试1：MPU6050连接测试 */
void Test_MPU6050_Connection(void)
{
if (MPU6050_TestConnection())
printf("MPU6050 Connected!\r\n");
else
printf("MPU6050 Not Found!\r\n");
}
/* 测试2：卡尔曼滤波效果测试 */
void Test_KalmanFilter(void)
{
MPU6050_Data data;
AngleData angle;
KalmanFilter kf_roll, kf_pitch;
Kalman_Init(&kf_roll);
Kalman_Init(&kf_pitch);
for (int i = 0; i < 100; i++)
{
MPU6050_ReadData(&data);
float accel_roll = atan2(data.ay, data.az) * 57.2958f;
float accel_pitch = atan2(-data.ax, sqrt(data.ay*data.ay + data.az*data.az)) * 57.2958f;
float gyro_roll = data.gx * 250.0f / 32768.0f;
float gyro_pitch = data.gy * 250.0f / 32768.0f;
float kalman_roll = Kalman_Update(&kf_roll, accel_roll, gyro_roll, 0.005f);
float kalman_pitch = Kalman_Update(&kf_pitch, accel_pitch, gyro_pitch, 0.005f);
printf("Raw Roll: %.2f, Raw Pitch: %.2f, Kalman Roll: %.2f, Kalman Pitch: %.2f\r\n",
accel_roll, accel_pitch, kalman_roll, kalman_pitch);
Delay_Ms(5);
}
}
4.2 调试输出示例
MPU6050 Connected!
Angle Data:
Roll: 15.2 deg, Pitch: -3.5 deg, Yaw: 0.0 deg
Roll Rate: 12.3 deg/s, Pitch Rate: -2.1 deg/s, Yaw Rate: 0.0 deg/s
----------------------------------------
Angle Data:
Roll: 15.3 deg, Pitch: -3.4 deg, Yaw: 0.1 deg
Roll Rate: 12.5 deg/s, Pitch Rate: -2.0 deg/s, Yaw Rate: 0.1 deg/s
----------------------------------------
4.3 MATLAB数据分析
% 读取CSV数据进行卡尔曼滤波效果分析
data = csvread('angle_data.csv');
timestamp = data(:,1);
raw_roll = data(:,2);
kalman_roll = data(:,3);
figure;
subplot(2,1,1);
plot(timestamp, raw_roll, 'r.', 'MarkerSize', 10);
hold on;
plot(timestamp, kalman_roll, 'b-', 'LineWidth', 2);
xlabel('Time (ms)');
ylabel('Roll Angle (deg)');
legend('Raw Data', 'Kalman Filtered');
title('Kalman Filter Effect on Roll Angle');
grid on;
subplot(2,1,2);
error = abs(raw_roll - kalman_roll);
plot(timestamp, error, 'g-', 'LineWidth', 1.5);
xlabel('Time (ms)');
ylabel('Estimation Error (deg)');
title('Kalman Filter Estimation Error');
grid on;
参考代码 基于STM32F103ZET6的对mpu6050的数据进行卡尔曼滤波最终得到精确的角度测量值 www.youwenfan.com/contentcsu/56418.html
五、优化建议
5.1 算法优化
- 自适应卡尔曼滤波：根据运动状态动态调整Q和R矩阵
- 扩展卡尔曼滤波（EKF）：处理非线性系统
- 无迹卡尔曼滤波（UKF）：更精确的协方差传播
- 多传感器融合：加入磁力计（电子罗盘）进行航向角校正
5.2 硬件优化
/* 振动补偿 */
void Vibration_Compensation(AngleData* angle)
{
// 检测高频振动
static float last_roll = 0;
float roll_change = fabs(angle->roll - last_roll);
if (roll_change > 10.0f) // 角度变化过大
{
// 降低卡尔曼增益，减少噪声影响
kalman_roll.R = 2.0f; // 增大观测噪声
}
else
{
kalman_roll.R = 0.5f; // 恢复正常
}
last_roll = angle->roll;
}
5.3 应用扩展
/* 姿态解算（四元数） */
typedef struct {
float q0, q1, q2, q3; // 四元数
} Quaternion;
/* 互补滤波（备用方案） */
float ComplementaryFilter(float accel_angle, float gyro_rate, float alpha)
{
static float angle = 0;
angle = alpha * (angle + gyro_rate * DT) + (1 - alpha) * accel_angle;
return angle;
}
