[stm32] MPU6050 HMC5883 Kalman 融合算法移植
一、卡尔曼滤波九轴融合算法stm32尝试
1、Kalman滤波文件[.h已经封装为结构体]
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics-> All rights reserved-> This software may be distributed and modified under the terms of the GNU
General Public License version 2 (GPL2) as published by the Free Software
Foundation and appearing in the file GPL2->TXT included in the packaging of
this file-> Please note that GPL2 Section 2[b] requires that all works based
on this software must also be made publicly available under the terms of
the GPL2 ("Copyleft")-> Contact information
------------------- Kristian Lauszus, TKJ Electronics
Web : http://www->tkjelectronics->com
e-mail : kristianl@tkjelectronics->com
*/ #ifndef _Kalman_h
#define _Kalman_h
struct Kalman {
/* Kalman filter variables */
double Q_angle; // Process noise variance for the accelerometer
double Q_bias; // Process noise variance for the gyro bias
double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise double angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector
double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate double P[][]; // Error covariance matrix - This is a 2x2 matrix
double K[]; // Kalman gain - This is a 2x1 vector
double y; // Angle difference
double S; // Estimate error
}; void Init(struct Kalman* klm){
/* We will set the variables like so, these can also be tuned by the user */
klm->Q_angle = 0.001;
klm->Q_bias = 0.003;
klm->R_measure = 0.03; klm->angle = ; // Reset the angle
klm->bias = ; // Reset bias klm->P[][] = ; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en->wikipedia->org/wiki/Kalman_filter#Example_application->2C_technical
klm->P[][] = ;
klm->P[][] = ;
klm->P[][] = ;
} // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
double getAngle(struct Kalman * klm, double newAngle, double newRate, double dt) {
// KasBot V2 - Kalman filter module - http://www->x-firm->com/?page_id=145
// Modified by Kristian Lauszus
// See my blog post for more information: http://blog->tkjelectronics->dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it // Discrete Kalman filter time update equations - Time Update ("Predict")
// Update xhat - Project the state ahead
/* Step 1 */
klm->rate = newRate - klm->bias;
klm->angle += dt * klm->rate; // Update estimation error covariance - Project the error covariance ahead
/* Step 2 */
klm->P[][] += dt * (dt*klm->P[][] - klm->P[][] - klm->P[][] + klm->Q_angle);
klm->P[][] -= dt * klm->P[][];
klm->P[][] -= dt * klm->P[][];
klm->P[][] += klm->Q_bias * dt; // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
// Calculate Kalman gain - Compute the Kalman gain
/* Step 4 */
klm->S = klm->P[][] + klm->R_measure;
/* Step 5 */
klm->K[] = klm->P[][] / klm->S;
klm->K[] = klm->P[][] / klm->S; // Calculate angle and bias - Update estimate with measurement zk (newAngle)
/* Step 3 */
klm->y = newAngle - klm->angle;
/* Step 6 */
klm->angle += klm->K[] * klm->y;
klm->bias += klm->K[] * klm->y; // Calculate estimation error covariance - Update the error covariance
/* Step 7 */
klm->P[][] -= klm->K[] * klm->P[][];
klm->P[][] -= klm->K[] * klm->P[][];
klm->P[][] -= klm->K[] * klm->P[][];
klm->P[][] -= klm->K[] * klm->P[][]; return klm->angle;
} void setAngle(struct Kalman* klm, double newAngle) { klm->angle = newAngle; } // Used to set angle, this should be set as the starting angle
double getRate(struct Kalman* klm) { return klm->rate; } // Return the unbiased rate /* These are used to tune the Kalman filter */
void setQangle(struct Kalman* klm, double newQ_angle) { klm->Q_angle = newQ_angle; }
void setQbias(struct Kalman* klm, double newQ_bias) { klm->Q_bias = newQ_bias; }
void setRmeasure(struct Kalman* klm, double newR_measure) { klm->R_measure = newR_measure; } double getQangle(struct Kalman* klm) { return klm->Q_angle; }
double getQbias(struct Kalman* klm) { return klm->Q_bias; }
double getRmeasure(struct Kalman* klm) { return klm->R_measure; } #endif
Kalman.h
2、I2C总线代码[这里把MPU和HMC挂接到上面，通过改变SlaveAddress的值来和不同的设备通信]
#include "stm32f10x.h" /*标志是否读出数据*/
char test=;
/*I2C从设备*/
unsigned char SlaveAddress;
/*模拟IIC端口输出输入定义*/
#define SCL_H GPIOB->BSRR = GPIO_Pin_10
#define SCL_L GPIOB->BRR = GPIO_Pin_10
#define SDA_H GPIOB->BSRR = GPIO_Pin_11
#define SDA_L GPIOB->BRR = GPIO_Pin_11
#define SCL_read GPIOB->IDR & GPIO_Pin_10
#define SDA_read GPIOB->IDR & GPIO_Pin_11 /*I2C的端口初始化---------------------------------------*/
void I2C_GPIO_Config(void)
{
GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
GPIO_Init(GPIOB, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
GPIO_Init(GPIOB, &GPIO_InitStructure);
} /*I2C的延时函数-----------------------------------------*/
void I2C_delay(void)
{
u8 i=; //这里可以优化速度 ，经测试最低到5还能写入
while(i)
{
i--;
}
} /*I2C的等待5ms函数--------------------------------------*/
void delay5ms(void)
{
int i=;
while(i)
{
i--;
}
} /*I2C启动函数-------------------------------------------*/
bool I2C_Start(void)
{
SDA_H;
SCL_H;
I2C_delay();
if(!SDA_read)return FALSE; //SDA线为低电平则总线忙,退出
SDA_L;
I2C_delay();
if(SDA_read) return FALSE; //SDA线为高电平则总线出错,退出
SDA_L;
I2C_delay();
return TRUE;
} /*I2C停止函数-------------------------------------------*/
void I2C_Stop(void)
{
SCL_L;
I2C_delay();
SDA_L;
I2C_delay();
SCL_H;
I2C_delay();
SDA_H;
I2C_delay();
} /*I2C的ACK函数------------------------------------------*/
void I2C_Ack(void)
{
SCL_L;
I2C_delay();
SDA_L;
I2C_delay();
SCL_H;
I2C_delay();
SCL_L;
I2C_delay();
} /*I2C的NoACK函数----------------------------------------*/
void I2C_NoAck(void)
{
SCL_L;
I2C_delay();
SDA_H;
I2C_delay();
SCL_H;
I2C_delay();
SCL_L;
I2C_delay();
} /*I2C等待ACK函数----------------------------------------*/
bool I2C_WaitAck(void) //返回为:=1有ACK,=0无ACK
{
SCL_L;
I2C_delay();
SDA_H;
I2C_delay();
SCL_H;
I2C_delay();
if(SDA_read)
{
SCL_L;
I2C_delay();
return FALSE;
}
SCL_L;
I2C_delay();
return TRUE;
} /*I2C发送一个u8数据函数---------------------------------*/
void I2C_SendByte(u8 SendByte) //数据从高位到低位//
{
u8 i=;
while(i--)
{
SCL_L;
I2C_delay();
if(SendByte&0x80)
SDA_H;
else
SDA_L;
SendByte<<=;
I2C_delay();
SCL_H;
I2C_delay();
}
SCL_L;
} /*I2C读取一个u8数据函数---------------------------------*/
unsigned char I2C_RadeByte(void) //数据从高位到低位//
{
u8 i=;
u8 ReceiveByte=; SDA_H;
while(i--)
{
ReceiveByte<<=;
SCL_L;
I2C_delay();
SCL_H;
I2C_delay();
if(SDA_read)
{
ReceiveByte|=0x01;
}
}
SCL_L;
return ReceiveByte;
} /*I2C向指定设备指定地址写入u8数据-----------------------*/
void Single_WriteI2C(unsigned char REG_Address,unsigned char REG_data)//单字节写入
{
if(!I2C_Start())return;
I2C_SendByte(SlaveAddress); //发送设备地址+写信号//I2C_SendByte(((REG_Address & 0x0700) >>7) | SlaveAddress & 0xFFFE);//设置高起始地址+器件地址
if(!I2C_WaitAck()){I2C_Stop(); return;}
I2C_SendByte(REG_Address ); //设置低起始地址
I2C_WaitAck();
I2C_SendByte(REG_data);
I2C_WaitAck();
I2C_Stop();
delay5ms();
} /*I2C向指定设备指定地址读出u8数据-----------------------*/
unsigned char Single_ReadI2C(unsigned char REG_Address)//读取单字节
{
unsigned char REG_data;
if(!I2C_Start())return FALSE;
I2C_SendByte(SlaveAddress); //I2C_SendByte(((REG_Address & 0x0700) >>7) | REG_Address & 0xFFFE);//设置高起始地址+器件地址
if(!I2C_WaitAck()){I2C_Stop();test=; return FALSE;}
I2C_SendByte((u8) REG_Address); //设置低起始地址
I2C_WaitAck();
I2C_Start();
I2C_SendByte(SlaveAddress+);
I2C_WaitAck(); REG_data= I2C_RadeByte();
I2C_NoAck();
I2C_Stop();
//return TRUE;
return REG_data;
}
I2C.c
3、MPU6050的驱动代码[updataMPU6050中获取数据为什么一正一负不是很清楚，是按照GitHub上arduino版本改的]
#define SlaveAddressMPU 0x68 //定义器件5883在IIC总线中的从地址 typedef unsigned char uchar; extern int accX, accY, accZ;
extern int gyroX, gyroY, gyroZ;
extern uchar SlaveAddress; //IIC写入时的地址字节数据，+1为读取
extern uchar Single_ReadI2C(uchar REG_Address); //读取I2C数据
extern void Single_WriteI2C(uchar REG_Address,uchar REG_data); //向I2C写入数据 //****************************************
// 定义MPU6050内部地址
//****************************************
#define SMPLRT_DIV 0x19 //陀螺仪采样率，典型值：0x07(125Hz)
#define CONFIG 0x1A //低通滤波频率，典型值：0x06(5Hz)
#define GYRO_CONFIG 0x1B //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
#define ACCEL_CONFIG 0x1C //加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)
#define ACCEL_XOUT_H 0x3B
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F
#define ACCEL_ZOUT_L 0x40
#define TEMP_OUT_H 0x41
#define TEMP_OUT_L 0x42
#define GYRO_XOUT_H 0x43
#define GYRO_XOUT_L 0x44
#define GYRO_YOUT_H 0x45
#define GYRO_YOUT_L 0x46
#define GYRO_ZOUT_H 0x47
#define GYRO_ZOUT_L 0x48
#define PWR_MGMT_1 0x6B //电源管理，典型值：0x00(正常启用)
#define WHO_AM_I 0x75 //IIC地址寄存器(默认数值0x68，只读)
#define MPU6050_Addr 0xD0 //IIC写入时的地址字节数据，+1为读取
//**************************************
//初始化MPU6050
//**************************************
void InitMPU6050()
{
SlaveAddress=MPU6050_Addr;
Single_WriteI2C(PWR_MGMT_1, 0x00); //解除休眠状态
Single_WriteI2C(SMPLRT_DIV, 0x07);// Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
Single_WriteI2C(CONFIG, 0x00);// Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
Single_WriteI2C(GYRO_CONFIG, 0x00);// Set Gyro Full Scale Range to ±250deg/s
Single_WriteI2C(ACCEL_CONFIG, 0x00);// Set Accelerometer Full Scale Range to ±2g
Single_WriteI2C(PWR_MGMT_1, 0x01);// PLL with X axis gyroscope reference and disable sleep mode
}
//**************************************
//// Get accelerometer and gyroscope values
//**************************************
void updateMPU6050()
{
SlaveAddress=MPU6050_Addr;// Get accelerometer and gyroscope values accX=((Single_ReadI2C(ACCEL_XOUT_H)<<)+Single_ReadI2C(ACCEL_XOUT_L));
accY=-((Single_ReadI2C(ACCEL_YOUT_H)<<)+Single_ReadI2C(ACCEL_YOUT_L));
accZ=((Single_ReadI2C(ACCEL_ZOUT_H)<<)+Single_ReadI2C(ACCEL_ZOUT_L)); gyroX=-((Single_ReadI2C(GYRO_XOUT_H)<<)+Single_ReadI2C(GYRO_XOUT_L));
gyroY=((Single_ReadI2C(GYRO_YOUT_H)<<)+Single_ReadI2C(GYRO_YOUT_L));
gyroZ=-((Single_ReadI2C(GYRO_ZOUT_H)<<)+Single_ReadI2C(GYRO_ZOUT_L));
}
MPU6050.c
4、HMC5883的驱动代码[updataHMC5883直接获取源数据，并未做大的处理]
#define uchar unsigned char
#define uint unsigned int //定义器件在IIC总线中的从地址,根据ALT ADDRESS地址引脚不同修改
#define HMC5883_Addr 0x3C //磁场传感器器件地址 unsigned char BUF[]; //接收数据缓存区
extern uchar SlaveAddress; //IIC写入时的地址字节数据，+1为读取 extern int magX, magY, magZ; //hmc最原始数据
extern uchar SlaveAddress; //IIC写入时的地址字节数据，+1为读取
extern uchar Single_ReadI2C(uchar REG_Address); //读取I2C数据
extern void Single_WriteI2C(uchar REG_Address,uchar REG_data); //向I2C写入数据
//**************************************
//初始化HMC5883，根据需要请参考pdf进行修改
//**************************************
void InitHMC5883()
{
SlaveAddress=HMC5883_Addr;
Single_WriteI2C(0x02,0x00); //
Single_WriteI2C(0x01,0xE0); //
}
//**************************************
//从HMC5883连续读取6个数据放在BUF中
//**************************************
void updateHMC5883()
{
SlaveAddress=HMC5883_Addr;
Single_WriteI2C(0x00,0x14);
Single_WriteI2C(0x02,0x00);
// Delayms(10); BUF[]=Single_ReadI2C(0x03);//OUT_X_L_A
BUF[]=Single_ReadI2C(0x04);//OUT_X_H_A
BUF[]=Single_ReadI2C(0x07);//OUT_Y_L_A
BUF[]=Single_ReadI2C(0x08);//OUT_Y_H_A
BUF[]=Single_ReadI2C(0x05);//OUT_Z_L_A
BUF[]=Single_ReadI2C(0x06);//OUT_Y_H_A magX=(BUF[] << ) | BUF[]; //Combine MSB and LSB of X Data output register
magY=(BUF[] << ) | BUF[]; //Combine MSB and LSB of Y Data output register
magZ=(BUF[] << ) | BUF[]; //Combine MSB and LSB of Z Data output register // if(magX>0x7fff)magX-=0xffff;//补码表示滴~所以要转化一下
// if(magY>0x7fff)magY-=0xffff;
// if(magZ>0x7fff)magZ-=0xffff;
}
HMC5883.c
5、USART简单的单字节发送的串口驱动文件
#include "stm32f10x.h" void USART1_Configuration(void);
void USART1_SendData(u8 SendData);
extern void Delayms(vu32 m); void USART1_Configuration()
{
GPIO_InitTypeDef GPIO_InitStructure;
USART_InitTypeDef USART_InitStructure;
USART_ClockInitTypeDef USART_ClockInitStructure; RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB ,ENABLE );//| RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOD, ENABLE );
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1 |RCC_APB2Periph_USART1, ENABLE ); /* Configure USART1 Tx (PA.09) as alternate function push-pull */
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9; // 选中管脚9
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; // 复用推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; // 最高输出速率50MHz
GPIO_Init(GPIOA, &GPIO_InitStructure); // 选择A端口 /* Configure USART1 Rx (PA.10) as input floating */
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10; //选中管脚10
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
GPIO_Init(GPIOA, &GPIO_InitStructure); //选择A端口 USART_ClockInitStructure.USART_Clock = USART_Clock_Disable; // 时钟低电平活动
USART_ClockInitStructure.USART_CPOL = USART_CPOL_Low; // 时钟低电平
USART_ClockInitStructure.USART_CPHA = USART_CPHA_2Edge; // 时钟第二个边沿进行数据捕获
USART_ClockInitStructure.USART_LastBit = USART_LastBit_Disable; // 最后一位数据的时钟脉冲不从SCLK输出
/* Configure the USART1 synchronous paramters */
USART_ClockInit(USART1, &USART_ClockInitStructure); // 时钟参数初始化设置 USART_InitStructure.USART_BaudRate = ; // 波特率为：115200
USART_InitStructure.USART_WordLength = USART_WordLength_8b; // 8位数据
USART_InitStructure.USART_StopBits = USART_StopBits_1; // 在帧结尾传输1个停止位
USART_InitStructure.USART_Parity = USART_Parity_No ; // 奇偶失能
USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; // 硬件流控制失能 USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; // 发送使能+接收使能
/* Configure USART1 basic and asynchronous paramters */
USART_Init(USART1, &USART_InitStructure); /* Enable USART1 */
USART_ClearFlag(USART1, USART_IT_RXNE); //清中断，以免一启用中断后立即产生中断
USART_ITConfig(USART1,USART_IT_RXNE, ENABLE); //使能USART1中断源
USART_Cmd(USART1, ENABLE); //USART1总开关：开启
}
void USART1_SendData(u8 SendData)
{
USART_SendData(USART1, SendData);
while(USART_GetFlagStatus(USART1, USART_FLAG_TC)==RESET);
}
USART.c
6、非精确延时函数集[其他文件所需的一些延时放在这里]
#include "stm32f10x.h" void Delay(vu32 nCount)
{
for(; nCount != ; nCount--);
}
void Delayms(vu32 m)
{
u32 i;
for(; m != ; m--)
for (i=; i<; i++);
}
DELAY.c
7、main函数文件
#include "stm32f10x.h"
#include "Kalman.h"
#include <math.h>
#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf struct Kalman kalmanX, kalmanY, kalmanZ; // Create the Kalman instances /* IMU Data MPU6050 AND HMC5883 Data*/
int accX, accY, accZ;
int gyroX, gyroY, gyroZ;
int magX, magY, magZ; double roll, pitch, yaw; // Roll and pitch are calculated using the accelerometer while yaw is calculated using the magnetometer double gyroXangle, gyroYangle, gyroZangle; // Angle calculate using the gyro only 只用陀螺仪计算角度
double compAngleX, compAngleY, compAngleZ; // Calculated angle using a complementary filter 用电磁计计算角度
double kalAngleX, kalAngleY, kalAngleZ; // Calculated angle using a Kalman filter 用kalman计算角度 //uint32_t timer,micros; //上一次时间与当前时间
uint8_t i2cData[]; // Buffer for I2C data #define MAG0MAX 603
#define MAG0MIN -578 #define MAG1MAX 542
#define MAG1MIN -701 #define MAG2MAX 547
#define MAG2MIN -556 #define RAD_TO_DEG 57.295779513082320876798154814105 // 弧度转角度的转换率
#define DEG_TO_RAD 0.01745329251994329576923690768489 // 角度转弧度的转换率 float magOffset[] = { (MAG0MAX + MAG0MIN) / , (MAG1MAX + MAG1MIN) / , (MAG2MAX + MAG2MIN) / };
double magGain[]; void SYSTICK_Configuration(void); //系统滴答中断配置
void RCC_Configuration(void);
void updatePitchRoll(void); //根据加速计刷新Pitch和Roll数据
void updateYaw(void); //根据磁力计刷新Yaw角
void InitAll(void); //循环前的初始化
void func(void); //循环里的内容
extern void InitMPU6050(void); //初始化MPU6050
extern void InitHMC5883(void); //初始化HMC5883
extern void updateMPU6050(void); //Get accelerometer and gyroscope values
extern void updateHMC5883(void); //Get magnetometer values
extern void USART1_Configuration(void); //串口初始化
extern void USART1_SendData(u8 SendData); //串口发送函数
extern void I2C_GPIO_Config(void); //I2C初始化函数
/****************************************************************************
* 名 称：int main(void)
* 功 能：主函数
* 入口参数：无
* 出口参数：无
* 说 明：
* 调用方法：无
****************************************************************************/
int main(void)
{
RCC_Configuration(); //系统时钟配置
USART1_Configuration();
I2C_GPIO_Config();
InitHMC5883();
InitMPU6050();
InitAll();
// SYSTICK_Configuration();
while()
{
func();
}
}
///*
//系统滴答中断配置
//*/
//void SYSTICK_Configuration(void)
//{
// micros=0;//全局计数时间归零
// if (SysTick_Config(72000)) //时钟节拍中断时1000ms一次 用于定时
// {
// /* Capture error */
//// while (1);
// }
//}
///*
//当前时间++.为了防止溢出当其大于2^20时，令其归零
//*/
//void SysTickHandler(void)
//{
// micros++;
// if(micros>(1<<20))
// micros=0;
//}
/****************************************************************************
* 名 称：void RCC_Configuration(void)
* 功 能：系统时钟配置为72MHZ
* 入口参数：无
* 出口参数：无
* 说 明：
* 调用方法：无
****************************************************************************/
void RCC_Configuration(void)
{
SystemInit();
} void InitAll()
{
/* Set Kalman and gyro starting angle */
updateMPU6050();
updateHMC5883();
updatePitchRoll();
updateYaw(); setAngle(&kalmanX,roll); // First set roll starting angle
gyroXangle = roll;
compAngleX = roll; setAngle(&kalmanY,pitch); // Then pitch
gyroYangle = pitch;
compAngleY = pitch; setAngle(&kalmanZ,yaw); // And finally yaw
gyroZangle = yaw;
compAngleZ = yaw; // timer = micros; // Initialize the timer
} void send(double xx,double yy,double zz)
{
int a[];
u8 i,sendData[];
a[]=(int)xx;a[]=(int)yy;a[]=(int)zz;
for(i=;i<;i++)
{
if(a[i]<){
sendData[i*]='-';
a[i]=-a[i];
}
else sendData[i*]=' ';
sendData[i*+]=(u8)(a[i]%/+0x30);
sendData[i*+]=(u8)(a[i]%/+0x30);
sendData[i*+]=(u8)(a[i]%+0x30);
}
for(i=;i<;i++)
{
USART1_SendData(sendData[i]);
}
USART1_SendData(0x0D);
USART1_SendData(0x0A);
} void func()
{
double gyroXrate,gyroYrate,gyroZrate,dt=0.01;
/* Update all the IMU values */
updateMPU6050();
updateHMC5883(); // dt = (double)(micros - timer) / 1000; // Calculate delta time
// timer = micros;
// if(dt<0)dt+=(1<<20); //时间是周期性的，有可能当前时间小于上次时间，因为这个周期远大于两次积分时间，所以最多相差1<<20 /* Roll and pitch estimation */
updatePitchRoll(); //用采集的加速计的值计算roll和pitch的值
gyroXrate = gyroX / 131.0; // Convert to deg/s 把陀螺仪的角加速度按照当初设定的量程转换为°/s
gyroYrate = gyroY / 131.0; // Convert to deg/s #ifdef RESTRICT_PITCH //如果上面有#define RESTRICT_PITCH就采用这种方法计算，防止出现-180和180之间的跳跃
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((roll < - && kalAngleX > ) || (roll > && kalAngleX < -)) {
setAngle(&kalmanX,roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
} else
kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter if (fabs(kalAngleX) > )
gyroYrate = -gyroYrate; // Invert rate, so it fits the restricted accelerometer reading
kalAngleY = getAngle(&kalmanY,pitch, gyroYrate, dt);
#else
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((pitch < - && kalAngleY > ) || (pitch > && kalAngleY < -)) {
kalmanY.setAngle(pitch);
compAngleY = pitch;
kalAngleY = pitch;
gyroYangle = pitch;
} else
kalAngleY = getAngle(&kalmanY, pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter if (abs(kalAngleY) > )
gyroXrate = -gyroXrate; // Invert rate, so it fits the restricted accelerometer reading
kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif /* Yaw estimation */
updateYaw();
gyroZrate = gyroZ / 131.0; // Convert to deg/s
// This fixes the transition problem when the yaw angle jumps between -180 and 180 degrees
if ((yaw < - && kalAngleZ > ) || (yaw > && kalAngleZ < -)) {
setAngle(&kalmanZ,yaw);
compAngleZ = yaw;
kalAngleZ = yaw;
gyroZangle = yaw;
} else
kalAngleZ = getAngle(&kalmanZ, yaw, gyroZrate, dt); // Calculate the angle using a Kalman filter /* Estimate angles using gyro only */
gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
gyroYangle += gyroYrate * dt;
gyroZangle += gyroZrate * dt;
//gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate from the Kalman filter
//gyroYangle += kalmanY.getRate() * dt;
//gyroZangle += kalmanZ.getRate() * dt; /* Estimate angles using complimentary filter */
compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
compAngleZ = 0.93 * (compAngleZ + gyroZrate * dt) + 0.07 * yaw; // Reset the gyro angles when they has drifted too much
if (gyroXangle < - || gyroXangle > )
gyroXangle = kalAngleX;
if (gyroYangle < - || gyroYangle > )
gyroYangle = kalAngleY;
if (gyroZangle < - || gyroZangle > )
gyroZangle = kalAngleZ; send(roll,pitch,yaw);
// send(gyroXangle,gyroYangle,gyroZangle);
// send(compAngleX,compAngleY,compAngleZ);
// send(kalAngleX,kalAngleY,kalAngleZ);
// send(kalAngleY,compAngleY,gyroYangle); /* Print Data */
// //#if 1
// printf("%lf %lf %lf %lf\n",roll,gyroXangle,compAngleX,kalAngleX);
// printf("%lf %lf %lf %lf\n",pitch,gyroYangle,compAngleY,kalAngleY);
// printf("%lf %lf %lf %lf\n",yaw,gyroZangle,compAngleZ,kalAngleZ);
//#endif // //#if 0 // Set to 1 to print the IMU data
// printf("%lf %lf %lf\n",accX / 16384.0,accY / 16384.0,accZ / 16384.0);
// printf("%lf %lf %lf\n",gyroXrate,gyroYrate,gyroZrate);
// printf("%lf %lf %lf\n",magX,magY,magZ);
//#endif //#if 0 // Set to 1 to print the temperature
//Serial.print("\t");
//
//double temperature = (double)tempRaw / 340.0 + 36.53;
//Serial.print(temperature); Serial.print("\t");
//#endif
// delay(10);
} //****************************************
//根据加速计刷新Pitch和Roll数据
//这里采用两种方法计算roll和pitch，如果最上面没有#define RESTRICT_PITCH就采用第二种计算方法
//****************************************
void updatePitchRoll() {
// Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
roll = atan2(accY,accZ) * RAD_TO_DEG;
pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
}
//****************************************
//根据磁力计刷新Yaw角
//****************************************
void updateYaw() { // See: http://www.freescale.com/files/sensors/doc/app_note/AN4248.pdf
double rollAngle,pitchAngle,Bfy,Bfx; magX *= -; // Invert axis - this it done here, as it should be done after the calibration
magZ *= -; magX *= magGain[];
magY *= magGain[];
magZ *= magGain[]; magX -= magOffset[];
magY -= magOffset[];
magZ -= magOffset[]; rollAngle = kalAngleX * DEG_TO_RAD;
pitchAngle = kalAngleY * DEG_TO_RAD; Bfy = magZ * sin(rollAngle) - magY * cos(rollAngle);
Bfx = magX * cos(pitchAngle) + magY * sin(pitchAngle) * sin(rollAngle) + magZ * sin(pitchAngle) * cos(rollAngle);
yaw = atan2(-Bfy, Bfx) * RAD_TO_DEG; yaw *= -;
}
main.c
程序说明：
int main(void)
{
RCC_Configuration(); //系统时钟配置
USART1_Configuration();
I2C_GPIO_Config();
InitHMC5883();
InitMPU6050();
InitAll();
// SYSTICK_Configuration();
while()
{
func();
}
}
- 主函数首先初始化系统时钟、串口、I2C总线、HMC5883磁力计和MPU6050加速计&陀螺仪，这里重点讲InitAll()函数和func()函数
void InitAll()
{
/* Set Kalman and gyro starting angle */
updateMPU6050();
updateHMC5883();
updatePitchRoll();
updateYaw(); setAngle(&kalmanX,roll); // First set roll starting angle
gyroXangle = roll;
compAngleX = roll; setAngle(&kalmanY,pitch); // Then pitch
gyroYangle = pitch;
compAngleY = pitch; setAngle(&kalmanZ,yaw); // And finally yaw
gyroZangle = yaw;
compAngleZ = yaw; // timer = micros; // Initialize the timer
}
- 第4、5两行从传感器中读取原数据，第6行函数根据加速计的值由空间几何的知识刷新Pitch和Roll数据，第7行函数根据复杂计算（这个实在看不懂，大概是磁力计有偏差，一方面进行误差校正，另一方面还用到了kalman滤波的数据，挺麻烦的）其实就是刷新yaw的值。
- 后面把kalman滤波值、陀螺仪计量值、磁力计计算值都赋值为上面计算的roll、pitch、yaw的值。
void func()
{
double gyroXrate,gyroYrate,gyroZrate,dt=0.01;
/* Update all the IMU values */
updateMPU6050();
updateHMC5883(); // dt = (double)(micros - timer) / 1000; // Calculate delta time
// timer = micros;
// if(dt<0)dt+=(1<<20); //时间是周期性的，有可能当前时间小于上次时间，因为这个周期远大于两次积分时间，所以最多相差1<<20 /* Roll and pitch estimation */
updatePitchRoll(); //用采集的加速计的值计算roll和pitch的值
gyroXrate = gyroX / 131.0; // Convert to deg/s 把陀螺仪的角加速度按照当初设定的量程转换为°/s
gyroYrate = gyroY / 131.0; // Convert to deg/s #ifdef RESTRICT_PITCH //如果上面有#define RESTRICT_PITCH就采用这种方法计算，防止出现-180和180之间的跳跃
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((roll < - && kalAngleX > ) || (roll > && kalAngleX < -)) {
setAngle(&kalmanX,roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
} else
kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter if (fabs(kalAngleX) > )
gyroYrate = -gyroYrate; // Invert rate, so it fits the restricted accelerometer reading
kalAngleY = getAngle(&kalmanY,pitch, gyroYrate, dt);
#else
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((pitch < - && kalAngleY > ) || (pitch > && kalAngleY < -)) {
kalmanY.setAngle(pitch);
compAngleY = pitch;
kalAngleY = pitch;
gyroYangle = pitch;
} else
kalAngleY = getAngle(&kalmanY, pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter if (abs(kalAngleY) > )
gyroXrate = -gyroXrate; // Invert rate, so it fits the restricted accelerometer reading
kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif /* Yaw estimation */
updateYaw();
gyroZrate = gyroZ / 131.0; // Convert to deg/s
// This fixes the transition problem when the yaw angle jumps between -180 and 180 degrees
if ((yaw < - && kalAngleZ > ) || (yaw > && kalAngleZ < -)) {
setAngle(&kalmanZ,yaw);
compAngleZ = yaw;
kalAngleZ = yaw;
gyroZangle = yaw;
} else
kalAngleZ = getAngle(&kalmanZ, yaw, gyroZrate, dt); // Calculate the angle using a Kalman filter /* Estimate angles using gyro only */
gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
gyroYangle += gyroYrate * dt;
gyroZangle += gyroZrate * dt;
//gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate from the Kalman filter
//gyroYangle += kalmanY.getRate() * dt;
//gyroZangle += kalmanZ.getRate() * dt; /* Estimate angles using complimentary filter */互补滤波算法
compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
compAngleZ = 0.93 * (compAngleZ + gyroZrate * dt) + 0.07 * yaw; // Reset the gyro angles when they has drifted too much
if (gyroXangle < - || gyroXangle > )
gyroXangle = kalAngleX;
if (gyroYangle < - || gyroYangle > )
gyroYangle = kalAngleY;
if (gyroZangle < - || gyroZangle > )
gyroZangle = kalAngleZ; send(roll,pitch,yaw);
// send(gyroXangle,gyroYangle,gyroZangle);
// send(compAngleX,compAngleY,compAngleZ);
// send(kalAngleX,kalAngleY,kalAngleZ);
// send(kalAngleY,compAngleY,gyroYangle);
}
- 5、6两行获取传感器原数据
- 8~10行计算两次测量的时间差dt[因为我采用很多方法试验来计算时间差都不奏效，所以最终还是放弃了这种算法，还是用我原来的DMP算法，DMP对水平方向的很好，z方向的不好，要用磁力计来纠正！可以参考这里面的算法！]
- 13~56行是用kalman滤波来求当前的3个角并稳值
- 60~62行是用陀螺仪的角速度积分获得当前陀螺仪测量的3个角度值
- 67~70行使用互补滤波算法对磁力计当前测量3个角的值进行计算
- 72~78行是稳值
- 81行是串口发送
PS:总的来说按照arduino的代码进行照抄移植成c语言版的，当前卡在了如何较为准确的计算dt，即：两次测量的时间差（这里为了测试我给了dt一个定值0.01s，这是很不准确的做法！！！）[我采用定时器的方法总是会莫名的跑偏，我想可能是中断的影响，好吧，还是用原来实验的DMP吧，这个算法看似高大上，其实比较占MCU的资源啦，自带的DMP也存在一些缺陷，对水平方向的偏角测量较为精准，误差在1°左右，而在z轴方向上的误差较大，容易跑偏，所以还要用磁力计进行纠正Z轴的测量值~]
PS:相关链接
- GitHub上面的基于arduino的工程：https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter.git
- 3轴加速计网页pdf版使用详细资料（公式，计算）：http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
- 加速计和磁力计倾斜补偿算法网页pdf资料：http://www.freescale.com/files/sensors/doc/app_note/AN4248.pdf
- 上述工程代码（你得自己解决dt问题）：http://pan.baidu.com/s/1gdlATFH
- MPU6050寄存器中文版：http://pan.baidu.com/s/1gdIKUK7
- MPU6050中文资料：http://pan.baidu.com/s/1bnkxjhP
- MPU6050数据轻松分析（基于arduino的kalman滤波讲解含代码）：http://pan.baidu.com/s/1eQvMtX4
- pitch yaw roll 相关知识(1)：http://blog.163.com/vipwdp@126/blog/static/150224366201281935518196/
- pitch yaw roll 相关知识(2)：http://www.cnblogs.com/wqj1212/archive/2010/11/21/1883033.html
- pitch yaw roll 相关知识(3)：http://www.cnblogs.com/tclikang/archive/2012/11/09/2761988.html
- 四元数与欧拉角知识：http://www.cnblogs.com/wqj1212/archive/2010/11/21/1883033.html
[stm32] MPU6050 HMC5883 Kalman 融合算法移植的更多相关文章
- Google Cardboard的九轴融合算法——基于李群的扩展卡尔曼滤波
Google Cardboard的九轴融合算法 --基于李群的扩展卡尔曼滤波 极品巧克力 前言 九轴融合算法是指通过融合IMU中的加速度计(三轴).陀螺仪(三轴).磁场计(三轴),来获取物体姿态的方法 ...
- 基于均值坐标(Mean-Value Coordinates)的图像融合算法的具体实现
目录 1. 概述 2. 实现 2.1. 准备 2.2. 核心 2.2.1. 均值坐标(Mean-Value Coordinates) 2.2.2. ROI边界栅格化 2.2.3. 核心实现 2.2.4 ...
- 基于均值坐标(Mean-Value Coordinates)的图像融合算法的优化实现
目录 1. 概述 2. 实现 2.1. 原理 2.2. 核心代码 2.3. 第二种优化 3. 结果 1. 概述 我在之前的文章<基于均值坐标(Mean-Value Coordinates)的图像 ...
- STM32 OSAL操作系统抽象层的移植
文章目录 什么是 OSAL? 源码安装 Linux 上OSAL的移植 STM32上OSAL的移植 关键点 测试代码 结语 附件 什么是 OSAL? 今天同学忽然问我有没有搞过OSAL,忽然间一头雾水, ...
- 一文搞懂 SLAM 中的Extension Kalman Filter 算法编程
作者 | Doreen 01 问题描述 预先知道事物未来的状态总是很有价值的! √ 预知台风的路线可以避免或减轻重大自然灾害的损失. √ 敌国打过来的导弹,如果能够高精度预测轨迹,就能有效拦截. √ ...
- paper 101：图像融合算法及视觉艺术应用
1:基于泊松方程的图像融合方法,利用偏微分方程实现了不同图像上区域的无缝融合.比较经典的文章: P. Pérez, M. Gangnet, A. Blake. Poisson image editin ...
- ROS下多雷达融合算法
有些小车车身比较长,如果是一个激光雷达,顾前不顾后,有比较大的视野盲区,这对小车导航定位避障来说都是一个问题,比如AGV小车, 所有想在小车前后各加一个雷达,那问题是ROS的建图或者定位导航都只是支持 ...
- [体感游戏] 1、MPU6050数据采集传输与可视化
最近在研究体感游戏,到目前为止实现了基于51单片机的MPU6050数据采集.利用蓝牙模块将数据传输到上位机,并利用C#自制串口数据高速采集软件,并且将数据通过自制的折线图绘制模块可视化地展示出来等功能 ...
- [Beautifulzzzz的博客目录] 快速索引点这儿O(∩_∩)O~~，红色标记的是不错的(⊙o⊙)哦~
3D相关开发 [direct-X] 1.direct-X最小框架 [OpenGL] 1.环境搭建及最小系统 [OpenGL] 2.企业版VC6.0自带的Win32-OpenGL工程浅析 51单片机 [ ...
随机推荐
- PHPRPC servlet发布服务
1.服务端 web.xml PHPRPCDispacherServlet 2.客户端 controller层调用
- EAS linux挂载数据盘
查看数据盘名称 fdisk -l 假设没有挂载的数据盘为/dev/xvdb 格式化数据盘 mkfs.ext3 /dev/xvdb 添加自动挂载 mkdir /data echo '/dev/xvdb ...
- jQuery触发<a>标签的点击事件无效
<a id="workFrame" href="pages/work.html" target="FrameBox">首页< ...
- Ruby-打包程序
1.下载 ocra-1.3.1.gem 文件 2.打开“start Command prompt with ruby” 命令窗口 3.输入 “gem install e:\ocra-1.3.1.ge ...
- ORA-12505 错误解决
在Fedora下安装了Oracle 10gR2,安装完成之后,使用netca创建了监听,创建的时候没有使用默认的LISTENER和1521端口,而是使用了LISTENER_DELL和1522端口,终 ...
- Json在前台与后台之间的使用
一.将前台数据,使用ajax中的post.get传到后台 $.ajax({ type: 'post', url: 'your url', data: $("form").seri ...
- Harris角点检测
代码示例一: #include<opencv2/opencv.hpp> using namespace cv; int main(){ Mat src = imread(); imshow ...
- apache的安装，启动和停止
一.apache服务器的安装 安装步骤直接傻瓜式进行安装.并没有太大的难点.apache的配置是学习的重点和难点. 安装好后再浏览器地址栏输入http://localhost.若能够成功安装,则会显示 ...
- Oracle 截取、查找字符函数（持续更新）
整理一些常用的截取.查找字符函数: 1.查找某一个字符串中某一个字符(串)出现的次数 SELECT LENGTH(REGEXP_REPLACE(REPLACE('anne<br>lily& ...
- C++通过模板实现多态
#include <iostream> using namespace std; template<typename T> class Phone { public: void ...