标题: 好不容易找的卡尔曼滤波的代码，分享给大家 [打印本页]
作者: happyfun    时间: 2016-7-23 13:09
标题: 好不容易找的卡尔曼滤波的代码，分享给大家
卡尔曼滤波算法

所有代码下载：
卡尔曼滤波代码.zip (2.89 KB, 下载次数: 114)
使用方法：
#include "kalman.h"

#include "stdio.h"

#include "stdlib.h"






void main(void)
{


    KalmanCountData k;
    //定义一个卡尔曼运算结构体
    Kalman_Filter_Init(&k);
    //讲运算变量初始化
    int m,n;        

       for(int a = 0;a<80;a++)
    //测试80次
    {

        //m,n为1到100的随机数
        m = 1+ rand() %100;

        n = 1+ rand() %100;

                //卡尔曼滤波，传递2个测量值以及运算结构体
   
    Kalman_Filter((float)m,(float)n,&k);

        //打印结果
        printf("%d and %d is %f - %f\r\n",m,n,k.Angle_Final,k.K_0);
   
    }




}

  1. /**
  2.   ******************************************************************************
  3.   * @file    kalman.h
  4.   * @author  willieon
  5.   * @version V0.1
  6.   * @date    January-2015
  7.   * @brief   卡尔曼滤波算法
  8.   *        
  9.   *
  10.   ******************************************************************************
  11.   * @attention
  12.   *本人对卡尔曼的粗略理解：以本次测量角速度（陀螺仪测量值）的积分得出的角度值
  13.   * 与上次最优角度值的方差产生一个权重来衡量本次测量角度（加速度测量值）
  14.   * 与上次最优角度值，从而产生新的最优角度值。好吧，比较拗口，有误处忘指正。
  15.   *
  16.   ******************************************************************************
  17.   */

  19. #ifndef __KALMAN_H__
  20. #define __KALMAN_H__


  23. #define Q_angle                        0.001                                ////角度过程噪声的协方差
  24. #define Q_gyro                        0.003                                ////角速度过程噪声的协方差
  25. #define R_angle                        0.5                                        ////测量噪声的协方差（即是测量偏差）
  26. #define dt                                0.01                                ////卡尔曼滤波采样频率
  27. #define C_0                                1

  29. /**************卡尔曼运算变量定义**********************
  30. *
  31. ***由于卡尔曼为递推运算，结构体需定义为全局变量
  32. ***在实际运用中只需定义一个KalmanCountData类型的变量即可
  33. ***无需用户定义多个中间变量，简化函数的使用
  34. */
  35. typedef struct
  36. {
  37.         float                                Q_bias;                                ////最优估计值的偏差，即估计出来的陀螺仪的漂移量
  38.         float                                Angle_err;                        ////实测角度与陀螺仪积分角度的差值
  39.         float                                PCt_0;                                
  40.         float                                PCt_1;
  41.         float                                E;                                        ////计算的过程量
  42.         float                                K_0;                                ////含有卡尔曼增益的另外一个函数，用于计算最优估计值
  43.         float                                K_1;                                ////含有卡尔曼增益的函数，用于计算最优估计值的偏差
  44.         float                                t_0;                                
  45.         float                                t_1;
  46.         float                                Pdot[4];                         ////Pdot[4] = {0,0,0,0};过程协方差矩阵的微分矩阵
  47.         float                                PP[2][2];                         //// PP[2][2] = { { 1, 0 },{ 0, 1 } };协方差(covariance)
  48.         float                                Angle_Final;                 ////后验估计最优角度值（即系统处理最终值）
  49.         float                                Gyro_Final;                         ////后验估计最优角速度值

  51. }KalmanCountData;

  53. void Kalman_Filter(float Accel,        float Gyro ,KalmanCountData * Kalman_Struct);
  54. void Kalman_Filter_Init(KalmanCountData * Kalman_Struct);



  58. #endif
复制代码


作者: lei787654    时间: 2017-9-26 12:16
看看，谢谢楼主
作者: 大猪蹄子    时间: 2019-1-17 15:43
谢谢楼主，下载了，先看看罗
作者: 梦园心田    时间: 2019-1-21 01:49
lei787654 发表于 2017-9-26 12:16
**** 作者被禁止或删除 内容自动屏蔽 ****

很好的东西，以后可能用得上
作者: jifengjianwu    时间: 2019-1-21 09:03
网上入门级的资料很多，哪里能找到稍微深入些的资料呢？
作者: wangxuewei    时间: 2021-1-7 14:16
共师兄习，共同进步
作者: 42151    时间: 2021-4-7 15:33
请问为何要调入角速度值？



欢迎光临 (http://www.51hei.com/bbs/) Powered by Discuz! X3.1