MPU6050 HMC5883 Kalman 融合算法移植

一、卡尔曼滤波九轴融合算法stm32尝试

1、Kalman滤波文件[.h已经封装为结构体]

1 /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics-> All rights reserved->

2

3 This software may be distributed and modified under the terms of the GNU

4 General Public License version 2 (GPL2) as published by the Free Software

5 Foundation and appearing in the file GPL2->TXT included in the packaging of

6 this file-> Please note that GPL2 Section 2[b] requires that all works based

7 on this software must also be made publicly available under the terms of

8 the GPL2 ("Copyleft")->

9

10 Contact information

11 -------------------

12

13 Kristian Lauszus, TKJ Electronics

14 Web : http://www->tkjelectronics->com

15 e-mail : kristianl@tkjelectronics->com

16 */

17

18 #ifndef _Kalman_h

19 #define _Kalman_h

20 struct Kalman {

21 /* Kalman filter variables */

22 double Q_angle; // Process noise variance for the accelerometer

23 double Q_bias; // Process noise variance for the gyro bias

24 double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise

25

26 double angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector

27 double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector

28 double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate

29

30 double P[2][2]; // Error covariance matrix - This is a 2x2 matrix

31 double K[2]; // Kalman gain - This is a 2x1 vector

32 double y; // Angle difference

33 double S; // Estimate error

34 };

35

36 void Init(struct Kalman* klm){

37 /* We will set the variables like so, these can also be tuned by the user */

38 klm->Q_angle = 0.001;

39 klm->Q_bias = 0.003;

40 klm->R_measure = 0.03;

41

42 klm->angle = 0; // Reset the angle

43 klm->bias = 0; // Reset bias

44

45 klm->P[0][0] = 0; // 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

46 klm->P[0][1] = 0;

47 klm->P[1][0] = 0;

48 klm->P[1][1] = 0;

49 }

50

51 // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds

52 double getAngle(struct Kalman * klm, double newAngle, double newRate, double dt) {

53 // KasBot V2 - Kalman filter module - http://www->x-firm->com/?page_id=145

54 // Modified by Kristian Lauszus

55 // See my blog post for more information: http://blog->tkjelectronics->dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it

56

57 // Discrete Kalman filter time update equations - Time Update ("Predict")

58 // Update xhat - Project the state ahead

59 /* Step 1 */

60 klm->rate = newRate - klm->bias;

61 klm->angle += dt * klm->rate;

62

63 // Update estimation error covariance - Project the error covariance ahead

64 /* Step 2 */

65 klm->P[0][0] += dt * (dt*klm->P[1][1] - klm->P[0][1] - klm->P[1][0] + klm->Q_angle);

66 klm->P[0][1] -= dt * klm->P[1][1];

67 klm->P[1][0] -= dt * klm->P[1][1];

68 klm->P[1][1] += klm->Q_bias * dt;

69

70 // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")

71 // Calculate Kalman gain - Compute the Kalman gain

72 /* Step 4 */

73 klm->S = klm->P[0][0] + klm->R_measure;

74 /* Step 5 */

75 klm->K[0] = klm->P[0][0] / klm->S;

76 klm->K[1] = klm->P[1][0] / klm->S;

77

78 // Calculate angle and bias - Update estimate with measurement zk (newAngle)

79 /* Step 3 */

80 klm->y = newAngle - klm->angle;

81 /* Step 6 */

82 klm->angle += klm->K[0] * klm->y;

83 klm->bias += klm->K[1] * klm->y;

84

85 // Calculate estimation error covariance - Update the error covariance

86 /* Step 7 */

87 klm->P[0][0] -= klm->K[0] * klm->P[0][0];

88 klm->P[0][1] -= klm->K[0] * klm->P[0][1];

89 klm->P[1][0] -= klm->K[1] * klm->P[0][0];

90 klm->P[1][1] -= klm->K[1] * klm->P[0][1];

91

92 return klm->angle;

93 }

94

95 void setAngle(struct Kalman* klm, double newAngle) { klm->angle = newAngle; } // Used to set angle, this should be set as the starting angle

96 double getRate(struct Kalman* klm) { return klm->rate; } // Return the unbiased rate

97

98 /* These are used to tune the Kalman filter */

99 void setQangle(struct Kalman* klm, double newQ_angle) { klm->Q_angle = newQ_angle; }

100 void setQbias(struct Kalman* klm, double newQ_bias) { klm->Q_bias = newQ_bias; }

101 void setRmeasure(struct Kalman* klm, double newR_measure) { klm->R_measure = newR_measure; }

102

103 double getQangle(struct Kalman* klm) { return klm->Q_angle; }

104 double getQbias(struct Kalman* klm) { return klm->Q_bias; }

105 double getRmeasure(struct Kalman* klm) { return klm->R_measure; }

106

2、I2C总线代码[这里把MPU和HMC挂接到上面，通过改变SlaveAddress的值来和不同的设备通信]

1 #include "stm32f10x.h"

2

3 /*标志是否读出数据*/

4 char test=0;

5 /*I2C从设备*/

6 unsigned char SlaveAddress;

7 /*模拟IIC端口输出输入定义*/

8 #define SCL_H GPIOB->BSRR = GPIO_Pin_10

9 #define SCL_L GPIOB->BRR = GPIO_Pin_10

10 #define SDA_H GPIOB->BSRR = GPIO_Pin_11

11 #define SDA_L GPIOB->BRR = GPIO_Pin_11

12 #define SCL_read GPIOB->IDR & GPIO_Pin_10

13 #define SDA_read GPIOB->IDR & GPIO_Pin_11

14

15 /*I2C的端口初始化---------------------------------------*/

16 void I2C_GPIO_Config(void)

17 {

18 GPIO_InitTypeDef GPIO_InitStructure;

19

20 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;

21 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

22 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;

23 GPIO_Init(GPIOB, &GPIO_InitStructure);

24

25 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;

26 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

27 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;

28 GPIO_Init(GPIOB, &GPIO_InitStructure);

29 }

30

31 /*I2C的延时函数-----------------------------------------*/

32 void I2C_delay(void)

33 {

34 u8 i=30; //这里可以优化速度 ，经测试最低到5还能写入

35 while(i)

36 {

37 i--;

38 }

39 }

40

41 /*I2C的等待5ms函数--------------------------------------*/

42 void delay5ms(void)

43 {

44 int i=5000;

45 while(i)

46 {

47 i--;

48 }

49 }

50

51 /*I2C启动函数-------------------------------------------*/

52 bool I2C_Start(void)

53 {

54 SDA_H;

55 SCL_H;

56 I2C_delay();

57 if(!SDA_read)return FALSE; //SDA线为低电平则总线忙,退出

58 SDA_L;

59 I2C_delay();

60 if(SDA_read) return FALSE; //SDA线为高电平则总线出错,退出

61 SDA_L;

62 I2C_delay();

63 return TRUE;

64 }

65

66 /*I2C停止函数-------------------------------------------*/

67 void I2C_Stop(void)

68 {

69 SCL_L;

70 I2C_delay();

71 SDA_L;

72 I2C_delay();

73 SCL_H;

74 I2C_delay();

75 SDA_H;

76 I2C_delay();

77 }

78

7