MPU6050 HMC5883 Kalman 融合算法移植
一、卡爾曼濾波九軸融合算法stm32嘗試
1、Kalman濾波文件[.h已經(jīng)封裝為結(jié)構(gòu)體]
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的值來和不同的設(shè)備通信]
1 #include "stm32f10x.h"
2
3 /*標(biāo)志是否讀出數(shù)據(jù)*/
4 char test=0;
5 /*I2C從設(shè)備*/
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的延時(shí)函數(shù)-----------------------------------------*/
32 void I2C_delay(void)
33 {
34 u8 i=30; //這里可以優(yōu)化速度 ，經(jīng)測試最低到5還能寫入
35 while(i)
36 {
37 i--;
38 }
39 }
40
41 /*I2C的等待5ms函數(shù)--------------------------------------*/
42 void delay5ms(void)
43 {
44 int i=5000;
45 while(i)
46 {
47 i--;
48 }
49 }
50
51 /*I2C啟動(dòng)函數(shù)-------------------------------------------*/
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線為高電平則總線出錯(cuò),退出
61 SDA_L;
62 I2C_delay();
63 return TRUE;
64 }
65
66 /*I2C停止函數(shù)-------------------------------------------*/
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
下載文檔
