九轴姿态传感器mpu6000

### 九轴姿态传感器 MPU6000 的使用方法及编程接口

#### 硬件连接
MPU6000 是一款集成三轴加速度计、三轴陀螺仪和三轴磁力计的九轴运动跟踪设备。为了将其与 STM32 微控制器相连，通常采用 I²C 或 SPI 接口进行通信。

对于 I²C 连接方式，需将 MPU6000 的 SDA 和 SCL 引脚分别接到 STM32 对应的 I²C 数据线和时钟线上；而 VCC 和 GND 则用于供电[^1]。

// 初始化IIC总线函数定义
void IIC_Init(void){
    // 配置GPIO端口模式为开漏输出, 上拉电阻外部提供
}

#### 编程驱动
初始化过程中要设置 MPU6000 工作参数，如量程范围、滤波带宽等，并开启所需传感器功能。具体操作可通过向特定寄存器写入命令完成：

- **GYRO_CONFIG** 寄存器控制陀螺仪灵敏度；
- **ACCEL_CONFIG** 设置加速计测量范围；
- **USER_CTRL** 可使能内部 DLPF (数字低通滤波)，提高数据稳定性[^2]。

#define MPU6000_ADDR         0xD0        /* 设备地址 */
#define SMPLRT_DIV           0x19        /* 输出速率分频 */

/* 向指定寄存器发送指令 */
static void mpu_write_reg(uint8_t reg,uint8_t data){
    HAL_I2C_Mem_Write(&hi2c1,&MPU6000_ADDR<<1,reg,I2C_MEMADD_SIZE_8BIT,&data,1,HAL_MAX_DELAY);
}

/* 开启DMP(Digital Motion Processor)*/
mpu_write_reg(0x6A,0x00);               // 清除睡眠位
mpu_write_reg(SMPLRT_DIV,0x07);          // 设置采样率=1kHz/(7+1)=125Hz
...

#### 数据处理
获取原始传感数值之后，还需经过一系列算法处理才能得到精确的角度信息。一般流程如下：
- 从 FIFO 中批量读取多组样本减少延迟误差影响；
- 应用卡尔曼滤波消除噪声干扰并融合不同类型的感知输入获得更可靠的结果；
- 计算欧拉角或四元数表示当前空间方位变化情况。

import numpy as np

def kalman_filter(z_measurements,P,Q,R,x_hat,A,H):
    """ 卡尔曼滤波预测更新 """
    ...
    
# 假设已知状态转移矩阵 A ,观测模型 H ，过程噪音 Q 测量方差 R 
kalman_params = {
    'P':np.eye(3),
    'Q':np.diag([0.01]*3),  
    'R':np.diag([0.1]*3)}