|
|
卡尔曼滤波算法
所有代码下载：
卡尔曼滤波代码.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);
}
}
- /**
- ******************************************************************************
- * @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
|
评分
-
查看全部评分
|
[复制链接]
QQ好友和群
QQ空间
腾讯微博
腾讯朋友
收藏
淘帖
顶
踩
