卡尔曼滤波 C语言实现

kalmen_filte.h

#ifndef  _KALMAN_FILTER_H
#define  _KALMAN_FILTER_H//1d卡尔曼滤波
typedef struct {float x;  // state float A;  // x(n)=A*x(n-1)+u(n),u(n)~N(0,q) float H;  // z(n)=H*x(n)+w(n),w(n)~N(0,r)   float q;  // process(predict) noise convariance 协方差float r;  // measure noise convariance float p;  // estimated error convariance 估计误差协方差float gain;
}kalman_struct;void kalman_init(kalman_struct *kalman_lcw, float init_x, float init_p);
float kalman_filter(kalman_struct *kalman_lcw, float z_measure);#endif  /*_KALMAN_FILTER_H*/

kalman_filte.c

#include "kalman_filter.h"/***kalman_init - 卡尔曼滤波器初始化*@kalman_lcw：卡尔曼滤波器结构体*@init_x：待测量的初始值*@init_p：后验状态估计值误差的方差的初始值*/
void kalman_init(kalman_struct *kalman_lcw, float init_x, float init_p)
{kalman_lcw->x = init_x;//待测量的初始值，如有中值一般设成中值（如陀螺仪）kalman_lcw->p = init_p;//后验状态估计值误差的方差的初始值kalman_lcw->A = 1;kalman_lcw->H = 1;kalman_lcw->q = 10e-2;//10e-6;//2e2;predict noise convariance 预测（过程）噪声方差 实验发现修改这个值会影响收敛速率kalman_lcw->r = 5e2;//10e-5;//测量（观测）噪声方差。以陀螺仪为例，测试方法是：//保持陀螺仪不动，统计一段时间内的陀螺仪输出数据。数据会近似正态分布，//按3σ原则，取正态分布的(3σ)^2作为r的初始化值
}/***kalman_filter - 卡尔曼滤波器*@kalman_lcw:卡尔曼结构体*@measure；测量值*返回滤波后的值*/
float kalman_filter(kalman_struct *kalman_lcw, float measure)
{/* Predict */kalman_lcw->x = kalman_lcw->A * kalman_lcw->x;kalman_lcw->p = kalman_lcw->A * kalman_lcw->A * kalman_lcw->p + kalman_lcw->q;  /* p(n|n-1)=A^2*p(n-1|n-1)+q *//* Measurement */kalman_lcw->gain = kalman_lcw->p * kalman_lcw->H / (kalman_lcw->p * kalman_lcw->H * kalman_lcw->H + kalman_lcw->r);kalman_lcw->x = kalman_lcw->x + kalman_lcw->gain * (measure - kalman_lcw->H * kalman_lcw->x);kalman_lcw->p = (1 - kalman_lcw->gain * kalman_lcw->H) * kalman_lcw->p;return kalman_lcw->x;
}

卡尔曼滤波 C语言实现相关推荐

【飞控理论】从零开始学习Kalman Filters之四：卡尔曼滤波C语言代码实现
文章目录前言学习目录 1.卡尔曼线性滤波的五条黄金公式 2.陀螺仪的原始数据 3.C语言源码分析附录 1.矩阵乘法 2.协方差矩阵 3.单位矩阵前言前面的文章系统介绍了卡尔曼滤波算法的数 ...
卡尔曼滤波C语言实现（矩阵版）
卡尔曼滤波算法及其C语言实现(矩阵操作版本) 卡尔曼滤波算法随机数的产生均匀分布随机数的产生正态分布的随机数矩阵的C语言实现卡尔曼滤波测试卡尔曼滤波算法这里就不详细讲解该算法,我觉得比较 ...
卡尔曼滤波--C语言实现
以下是一段用C语言实现的卡尔曼滤波算法,供有需要的小伙伴参考 #include "stdio.h" #include "stdlib.h" #include & ...
卡尔曼滤波 c语言模板
卡尔曼滤波算法 C语言代码编写如下所示 #include <math.h> #include <stdio.h>/*R值固定,Q值越大,代表越信任测量值,Q值无穷大,代表只 ...
卡尔曼滤波C语言实现
卡尔曼5条基本公式,参考https://wenku.baidu.com/view/8523cb6eaf1ffc4ffe47ac24.html #include "stdio.h" ...
【飞控理论】从零开始学习Kalman Filters之三：非线性状态估算器
文章目录前言学习目录 1.开始之前先需要知道的几个概念 2.非线性状态估算器 2.1.扩展卡尔曼滤波器(EKF) 2.2.无味卡尔曼滤波器(UKF) 2.3.粒子滤波器(PF) 2.4.总表 ...
【飞控理论】从零开始学习Kalman Filters之二：最优状态估计、最优估计算法和方程
文章目录前言学习目录 1.开始之前先需要知道的几个概念 2.最优状态估计 3.最优估计算法和方程本文是博主学习笔记,点击这里观看原视频. 前言在上次的学习中我们学习了卡尔曼滤波器的常见 ...
【飞控理论】从零开始学习Kalman Filters之一：Kalman Filters的常见用途、什么是状态观测器？
文章目录前言学习目录 1.Kalman Filters的常见用途 1.1.数据源无法直接被测量时卡尔曼滤波器是如何进行估算系统的状态 1.2.数据源在噪音影响下如何使用卡尔曼滤波器来估计系统的状态 ...
卡尔曼滤波算法c语言stm32,卡尔曼滤波算法及C语言实现_源代码
a往南向北 2019-01-16 20:39:20 11340 收藏 111 分类专栏: C语言嵌入式文章标签: 卡尔曼滤波 C代码卡尔曼滤波理论很容易就可以在MATLAB软件环境下实现,但是 ...

卡尔曼滤波 C语言实现

卡尔曼滤波 C语言实现相关推荐

最新文章

热门文章