一、卡尔曼滤波九轴融合算法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 107 #endif