发 帖  
[经验] STM32可以用的卡尔曼滤波,附带AHRS姿态解算源码
2015-6-12 13:57:22  12973
分享
经测试,在STM32上面可以用的卡尔曼滤波程序,效果的话,有一定的滤波效果,大家拿去参考吧
kalman.zip (4.89 KB, 下载次数: 762)


还有我现在学习姿态解算找到的,德国的AHRS姿态解算源码,很有参考价值
  1.     /*
  2.     * AHRS
  3.     * Copyright 2010 S.O.H. Madgwick
  4.     *
  5.     * This program is free software: you can redistribute it and/or
  6.     * modify it under the terms of the GNU Lesser Public License as
  7.     * published by the Free Software Foundation, either version 3 of the
  8.     * License, or (at your option) any later version.
  9.     *
  10.     * This program is distributed in the hope that it will be useful, but
  11.     * WITHOUT ANY WARRANTY; without even the implied warranty of
  12.     * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13.     * Lesser Public License for more details.
  14.     *
  15.     * You should have received a copy of the GNU Lesser Public License
  16.     * along with this program.  If not, see
  17.     * <http://www.gnu.org/licenses/>.
  18.     */
  19.     // AHRS.c
  20.     // S.O.H. Madgwick
  21.     // 25th August 2010
  22.     //
  23.     // Quaternion implementation of the 'DCM filter' [Mayhony et al].  Incorporates the magnetic distortion
  24.     // compensation algorithms from my filter [Madgwick] which eliminates the need for a reference
  25.     // direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
  26.     // axis only.
  27.     //
  28.     // User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
  29.     //
  30.     // Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
  31.     // orientation.  See my report for an overview of the use of quaternions in this application.
  32.     //
  33.     // User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'),
  34.     // accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data.  Gyroscope units are
  35.     // radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised.
  36.     //

  37.                                                                                     
  38.     #include "stm32f10x.h"                 
  39.     #include "AHRS.h"
  40.     #include "Positioning.h"      
  41.     #include <math.h>                          
  42.     #include <stdio.h>



  43.     /* Private define ------------------------------------------------------------*/
  44.     #define Kp 2.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
  45.     #define Ki 0.005f                // integral gain governs rate of convergence of gyroscope biases
  46.     #define halfT 0.0025f                // half the sample period        : 0.005s/2=0.0025s

  47.     #define ACCEL_1G 1000 //the acceleration of gravity is: 1000 mg

  48.     /* Private variables ---------------------------------------------------------*/
  49.     static float q0 = 1, q1 = 0, q2 = 0, q3 = 0;        // quaternion elements representing the estimated orientation
  50.     static float exInt = 0, eyInt = 0, ezInt = 0;        // scaled integral error

  51.     /* Public variables ----------------------------------------------------------*/
  52.     EulerAngle_Type EulerAngle;        //unit: radian
  53.     u8 InitEulerAngle_Finished = 0;

  54.     float Magnetoresistor_mGauss_X = 0, Magnetoresistor_mGauss_Y = 0, Magnetoresistor_mGauss_Z = 0;//unit: milli-Gauss                                                                                                                                                                                                      
  55.     float Accelerate_mg_X, Accelerate_mg_Y, Accelerate_mg_Z;//unit: mg                                                               
  56.     float AngularRate_dps_X, AngularRate_dps_Y, AngularRate_dps_Z;//unit: dps: degree per second      

  57.     int16_t Magnetoresistor_X, Magnetoresistor_Y, Magnetoresistor_Z;                                                                                                                                                                                                      
  58.     uint16_t Accelerate_X = 0, Accelerate_Y = 0, Accelerate_Z = 0;                                                                                                                                                                                               
  59.     uint16_t AngularRate_X = 0, AngularRate_Y = 0, AngularRate_Z = 0;

  60.     u8 Quaternion_Calibration_ok = 0;

  61.     /* Private macro -------------------------------------------------------------*/
  62.     /* Private typedef -----------------------------------------------------------*/
  63.     /* Private function prototypes -----------------------------------------------*/

  64.     /*******************************************************************************
  65.     * Function Name  : AHRSupdate
  66.     * Description    : None
  67.     * Input          : None
  68.     * Output         : None
  69.     * Return         : None
  70.     *******************************************************************************/
  71.     void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
  72.             float norm;
  73.             float hx, hy, hz, bx, bz;
  74.             float vx, vy, vz, wx, wy, wz;
  75.             float ex, ey, ez;

  76.             // auxiliary variables to reduce number of repeated operations
  77.             float q0q0 = q0*q0;
  78.             float q0q1 = q0*q1;
  79.             float q0q2 = q0*q2;
  80.             float q0q3 = q0*q3;
  81.             float q1q1 = q1*q1;
  82.             float q1q2 = q1*q2;
  83.             float q1q3 = q1*q3;
  84.             float q2q2 = q2*q2;   
  85.             float q2q3 = q2*q3;
  86.             float q3q3 = q3*q3;         
  87.            
  88.             // normalise the measurements
  89.             norm = sqrt(ax*ax + ay*ay + az*az);      
  90.             ax = ax / norm;
  91.             ay = ay / norm;
  92.             az = az / norm;
  93.             norm = sqrt(mx*mx + my*my + mz*mz);         
  94.             mx = mx / norm;
  95.             my = my / norm;
  96.             mz = mz / norm;         
  97.            
  98.             // compute reference direction of flux
  99.             hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
  100.             hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
  101.             hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);         
  102.             bx = sqrt((hx*hx) + (hy*hy));
  103.             bz = hz;        
  104.            
  105.             // estimated direction of gravity and flux (v and w)
  106.             vx = 2*(q1q3 - q0q2);
  107.             vy = 2*(q0q1 + q2q3);
  108.             vz = q0q0 - q1q1 - q2q2 + q3q3;
  109.             wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
  110.             wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
  111.             wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  
  112.            
  113.             // error is sum of cross product between reference direction of fields and direction measured by sensors
  114.             ex = (ay*vz - az*vy) + (my*wz - mz*wy);
  115.             ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
  116.             ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
  117.            
  118.             // integral error scaled integral gain
  119.             exInt = exInt + ex*Ki;
  120.             eyInt = eyInt + ey*Ki;
  121.             ezInt = ezInt + ez*Ki;
  122.            
  123.             // adjusted gyroscope measurements
  124.             gx = gx + Kp*ex + exInt;
  125.             gy = gy + Kp*ey + eyInt;
  126.             gz = gz + Kp*ez + ezInt;
  127.            
  128.             // integrate quaternion rate and normalise
  129.             q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
  130.             q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
  131.             q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
  132.             q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  
  133.            
  134.             // normalise quaternion
  135.             norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  136.             q0 = q0 / norm;
  137.             q1 = q1 / norm;
  138.             q2 = q2 / norm;
  139.             q3 = q3 / norm;
  140.     }

  141.     float clamp(float Value, float Min, float Max)
  142.     {
  143.             if(Value > Max)
  144.             {
  145.                     return Max;
  146.             }else if(Value < Min)
  147.             {
  148.                     return Min;
  149.             }else
  150.             {
  151.                     return Value;
  152.             }
  153.     }
复制代码



14
2015-6-12 13:57:22   评论
78 个讨论
好资料,谢谢分享
2015-6-19 20:57:05 评论

举报

thank you very much.
2015-6-25 03:41:14 评论

举报

学习记号备用,谢谢分享
2015-6-26 16:57:18 评论

举报

3Q......................................
2015-6-27 23:37:52 评论

举报

学习中。。。。。
2015-6-30 23:00:42 评论

举报

好资料,谢谢分享
2015-7-19 19:35:12 评论

举报

感谢分享                                                                                         
2015-8-2 00:22:26 评论

举报

好东西,谢楼主了
谢谢楼主分享,卡尔曼滤波一直不懂,最近在钻研中。
2015-8-6 16:21:10 评论

举报

谢谢分享                       !!!!!!!!!!!!!!!!!!
2015-8-24 16:28:31 评论

举报

谢谢分享谢谢分享谢谢分享
      谢谢分享谢谢分享                       谢谢分享
      谢谢分享谢谢分享                谢谢分享         
            谢谢分享              谢谢分享谢谢分享谢谢分享
            谢谢分享              谢谢分享        谢                谢
            谢谢分享              谢谢分享        谢                谢
            谢谢分享              谢谢分享        分                分
            谢谢分享              谢谢分享        享                享
            谢谢分享              谢谢分享        谢                谢
            谢谢分享              谢谢分享        谢                谢
谢        谢谢分享               谢谢分享       分                分
谢谢     谢谢分享              谢谢分享        享                享
谢谢分 谢谢分享              谢谢分享        谢                谢
谢谢分 谢谢分享                               谢       谢
谢谢分谢谢分享                         谢                 谢
        谢谢谢谢                    分                               分
2015-8-24 16:48:45 评论

举报

谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢谢下
2015-10-20 00:33:50 评论

举报

谢谢分享,好东西,值得下载!
2015-10-22 09:37:36 评论

举报

xiexielouzhu  学习中!!!!!!!!!
2015-11-21 11:50:35 评论

举报

粗略看了一下程序,写的蛮规范的。
2015-11-24 13:14:27 评论

举报

努力学习中。。。。。。。
2015-12-10 15:47:00 评论

举报

好好好好好好好好好好好好好好好好好好好好好好好好好好好好好好好好好好好好好好
2015-12-16 21:01:28 评论

举报

顶!!!!!!!!!!!!
2016-1-6 15:34:53 评论

举报

顶!!!!!!!!!!!!
2016-1-7 09:23:45 评论

举报

高级模式
您需要登录后才可以回帖 登录 | 注册

1234下一页
发表新帖
关闭

站长推荐 上一条 /7 下一条

快速回复 返回顶部 返回列表