最近研究STM32的自平衡小车，发现有两座必过的大山，一为卡尔曼滤波，二为PID算法。
网上看了很多关于卡尔曼滤波的代码，感觉写得真不咋地。一怒之下，自己重写，不废话，贴代码

[pre lang="C" line="1" file="kalman.h"]/**
  ******************************************************************************
  * @file    kalman.h
  * @author  willieon
  * @version V0.1
  * @date    January-2015
  * @brief   卡尔曼滤波算法 
  *        
  *
  ******************************************************************************
  * @attention
  *本人对卡尔曼的粗略理解：以本次测量角速度（陀螺仪测量值）的积分得出的角度值
  * 与上次最优角度值的方差产生一个权重来衡量本次测量角度（加速度测量值）
  * 与上次最优角度值，从而产生新的最优角度值。好吧，比较拗口，有误处忘指正。
  *
  ******************************************************************************
  */

#ifndef __KALMAN_H__
#define __KALMAN_H__

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

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

}KalmanCountData;

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

#endif

[/pre]

kalman.c

[pre lang="C" line="1"  file="kalman.c"]
#include "kalman.h"

/**
  ******************************************************************************
  * @file    void Kalman_Filter_Init(KalmanCountData * Kalman_Struct)
  * @author  willieon
  * @version V0.1
  * @date    January-2015
  * @brief   卡尔曼滤波计算中间量初始化
  *        
  *
  ******************************************************************************
  * @attention
  *
  * 
  * 
  *
  ******************************************************************************
  */

void Kalman_Filter_Init(KalmanCountData * Kalman_Struct)
{
        Kalman_Struct -> Angle_err                 = 0;
        Kalman_Struct -> Q_bias                         = 0;
        Kalman_Struct -> PCt_0                         = 0;
        Kalman_Struct -> PCt_1                         = 0;
        Kalman_Struct -> E                                 = 0;
        Kalman_Struct -> K_0                         = 0;
        Kalman_Struct -> K_1                         = 0;
        Kalman_Struct -> t_0                         = 0;
        Kalman_Struct -> t_1                         = 0;
        Kalman_Struct -> Pdot[0]                 = 0;
        Kalman_Struct -> Pdot[1]                 = 0;
        Kalman_Struct -> Pdot[2]                 = 0;
        Kalman_Struct -> Pdot[3]                 = 0;        
        Kalman_Struct -> PP[0][0]                 = 1;
        Kalman_Struct -> PP[0][1]                 = 0;
        Kalman_Struct -> PP[1][0]                 = 0;
        Kalman_Struct -> PP[1][1]                 = 1;        
        Kalman_Struct -> Angle_Final         = 0;
        Kalman_Struct -> Gyro_Final                 = 0;

}

/**
  ******************************************************************************
  * @file    void Kalman_Filter(float Accel,        float Gyro ,KalmanCountData * Kalman_Struct)
  * @author  willieon
  * @version V0.1
  * @date    January-2015
  * @brief   卡尔曼滤波计算
  *        
  *
  ******************************************************************************
  * @attention
  *                Accel:加速度计数据处理后进来的角度值
  *                Gyro :陀螺仪数据处理后进来的角速度值
  *                Kalman_Struct:递推运算所需要的中间变量，由用户定义为全局结构体变量
  *                Kalman_Struct -> Angle_Final  为滤波后角度最优值
  *                Kalman_Struct -> Gyro_Final   为后验角度值
  ******************************************************************************
  */

void Kalman_Filter(float Accel,        float Gyro ,KalmanCountData * Kalman_Struct)
{
                //陀螺仪积分角度（先验估计）
                Kalman_Struct -> Angle_Final += (Gyro - Kalman_Struct -> Q_bias) * dt; 
                
                //先验估计误差协方差的微分
                Kalman_Struct -> Pdot[0] = Q_angle - Kalman_Struct -> PP[0][1] - Kalman_Struct -> PP[1][0]; 
                Kalman_Struct -> Pdot[1] = - Kalman_Struct -> PP[1][1];
                Kalman_Struct -> Pdot[2] = - Kalman_Struct -> PP[1][1];
                Kalman_Struct -> Pdot[3] = Q_gyro;
                
                //先验估计误差协方差的积分
                Kalman_Struct -> PP[0][0] += Kalman_Struct -> Pdot[0] * dt;   
                Kalman_Struct -> PP[0][1] += Kalman_Struct -> Pdot[1] * dt;   
                Kalman_Struct -> PP[1][0] += Kalman_Struct -> Pdot[2] * dt;
                Kalman_Struct -> PP[1][1] += Kalman_Struct -> Pdot[3] * dt;
                
                //计算角度偏差
                Kalman_Struct -> Angle_err = Accel - Kalman_Struct -> Angle_Final;        
                
                //卡尔曼增益计算
                Kalman_Struct -> PCt_0 = C_0 * Kalman_Struct -> PP[0][0];
                Kalman_Struct -> PCt_1 = C_0 * Kalman_Struct -> PP[1][0];
                
                Kalman_Struct -> E = R_angle + C_0 * Kalman_Struct -> PCt_0;
                
                Kalman_Struct -> K_0 = Kalman_Struct -> PCt_0 / Kalman_Struct -> E;
                Kalman_Struct -> K_1 = Kalman_Struct -> PCt_1 / Kalman_Struct -> E;
                
                //后验估计误差协方差计算
                Kalman_Struct -> t_0 = Kalman_Struct -> PCt_0;
                Kalman_Struct -> t_1 = C_0 * Kalman_Struct -> PP[0][1];

Kalman_Struct -> PP[0][0] -= Kalman_Struct -> K_0 * Kalman_Struct -> t_0;                 
                Kalman_Struct -> PP[0][1] -= Kalman_Struct -> K_0 * Kalman_Struct -> t_1;
                Kalman_Struct -> PP[1][0] -= Kalman_Struct -> K_1 * Kalman_Struct -> t_0;
                Kalman_Struct -> PP[1][1] -= Kalman_Struct -> K_1 * Kalman_Struct -> t_1;

Kalman_Struct -> Angle_Final += Kalman_Struct -> K_0 * Kalman_Struct -> Angle_err;         //后验估计最优角度值
                Kalman_Struct -> Q_bias        += Kalman_Struct -> K_1 * Kalman_Struct -> Angle_err;                 //更新最优估计值的偏差
                Kalman_Struct -> Gyro_Final   = Gyro - Kalman_Struct -> Q_bias;                                                 //更新最优角速度值

}

[/pre]

代码可以放在实际工程中使用，也可以用VS等C编译工具进行实验学习。在VS中的main()实例使用如下

[pre lang="C" line="1" file="main.c"]
#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);
        
        }

}
[/pre]

平衡小车卡尔曼滤波算法相关推荐

  1. matlab两轮自平衡小车,基于LQR算法两轮自平衡小车的系统设计与研究

    摘要: 本文旨在设计和研究两轮自平衡小车系统.两轮自平衡小车是一种非线性.强耦合.多变量.自然不稳定.具体的.实现起来相对便宜的复杂系统,给控制理论提出了很大的挑战,是检验各种控制方法处理能力的典型装 ...

  2. PID算法控制平衡小车直立

    1.平衡小车直立控制: 如果我们要控制一根木棍在手上直立,需要两个步骤 ->托着木棒的手可以移动 ->眼睛能看到木棒的倾斜角度和倾斜趋势(角速度) 类比到平衡小车中,同理想让小车保持平衡, ...

  3. PID算法控制平衡小车速度

    1.平衡小车速度控制: 平衡小车在平衡的前提下,应该想办法让他直立行走,也就是下面要讲的在直立控制下给予小车速度达到速度控制 速度控制应该控制的是倾斜角从而触发直立控制达到移动的目的,小车运动的速度和 ...

  4. Arduino平衡小车

    Arduino平衡小车 1.概述 此Arduino平衡小车在主控方面由Arduino UNO R3和Arduino sensor shield v5.0传感器扩展板组成.采用TB6612FNG作为电源 ...

  5. 基于STM32-F401的平衡小车

    目录 一.控制系统设计 1.1机械结构设计 1.2传感系统设计 1.3执行器设计 1.4控制算法设计 二.控制系统的制作与调试 2.1机械结构的制作与调试 2.2电路系统的制作与调试 2.3控制程序的 ...

  6. matlab两轮自平衡小车,(2-3合刊) 基于MEMS惯性传感器的两轮自平衡小车设计

    摘要:着重分析了两轮自平衡小车的设计原理与控制算法,采用卡尔曼滤波算法融合陀螺仪与加速度计信号,得到系统姿态倾角与角速度最优估计值,通过双闭环数字PID 算法实现系统的自平衡控制.设计了以MPU-60 ...

  7. 基于STM32的二轮自平衡小车

    前言 近年来,移动机器人是目前科学领域比较活跃的领域之一,其应用范围越来越广泛,面临的环境也越来越复杂,这就要求机器人能够适应一些复杂的环境和任务.二轮自平衡机器人正是在这一背景下提出来的,对于制作此 ...

  8. 关于STM32平衡小车的几个关键疑问

    引用文章 一.关于姿态融合 首先需要提一点关键的,**如果是采用卡尔曼滤波或者互补滤波,尽可能把采样时间调小,这样可以增加滤波的效果.建议采样时间不要高于10ms,起初我在测试的时候采样时间设置为20 ...

  9. 毕业论文 | 基于STM32的双轮平衡小车设计（基于Keil5的完整注释版代码工程，原器件清单）

    博主github:https://github.com/MichaelBeechan 博主CSDN:https://blog.csdn.net/u011344545 预告:代码及文档下载 其他参考代码 ...

最新文章

  1. 前端开发知识总结思维导图
  2. IT人保持健康的必备法宝
  3. 创建docker用户组并加入
  4. php.ini文件可以复制吗,php安装完成以后要复制php.ini文件
  5. Web.Config介绍
  6. 使用Hybris Commerce User API读取用户信息时，电话字段没有返回
  7. （翻译）Tricks of the windows game programming Gurus(Windows游戏大师之路) --- 前言(作者：ANDRE LAMOTHE 1999)...
  8. 单元测试(UT)、功能测试(FT)(转)
  9. redchat怎么编写shell脚本_Linux如何编写shell脚本?
  10. socket通信压力测试
  11. 罗德矢量网络分析仪高效测试软件NSAT-1000
  12. 开启新坑，将live2d引入网页
  13. 董文永武汉大学计算机学院,董文永
  14. C语言数字图像处理---2.5图像频域滤波
  15. 使用for循环编写反方向正直角三角形
  16. 「 墙裂推荐」互联网人必备GIF制作的14种选择
  17. 你应该如何学习一个未知的技术领域？- 菜鸟小白篇
  18. 葡萄酒数据集_如何使用数据科学来理解什么使葡萄酒味道更好
  19. win10计算机右键属性打不开,win10电脑系统属性打不开的解决方法
  20. 关于C++ delete 来释放new分配的内存

热门文章

  1. IPC 中 LPC、RPC 的区别和联系
  2. 计算机程序设计艺术+第3卷：排序与查找（第二版）pdf
  3. IE8兼容问题总结---trim()方法
  4. [BZOJ 3236] [Ahoi2013] 作业 [BZOJ 3809] 【莫队（+分块）】
  5. 如何在WinForm中发送HTTP请求
  6. Thinking in C++遇到的函数指针及应用
  7. java操作elasticsearch实现前缀查询、wildcard、fuzzy模糊查询、ids查询
  8. 大数据分析如何创建最佳的移动应用用户体验
  9. [QNAP crontab 定時執行程式
  10. 研究人员发现Office Word 0Day攻击 这个漏洞绕过了word宏安全设置 绿盟科技、McAfee及FireEye发出警告...