先上代码
https://github.com/blueskit/KalmanFilter
namespace FusionFiltering { /// <summary> /// Simple implementation of the Kalman Filter for 1D data. /// Originally written in JavaScript by Wouter Bulten /// /// https://github.com/wouterbulten/kalmanjs/blob/master/contrib/java/KalmanFilter.java /// </summary> public class FilterKalman { private double A = 1; private double B = 0; private double C = 1; private double R; private double Q; private double cov = double.NaN; private double x = double.NaN; /// <summary> /// Constructor /// </summary> /// <param name="R">R Process noise</param> /// <param name="Q">Q Measurement noise</param> /// <param name="A">A State vector</param> /// <param name="B">B Control vector</param> /// <param name="C">C Measurement vector</param> public FilterKalman(double R, double Q, double A, double B, double C) { this.R = R; this.Q = Q; this.A = A; this.B = B; this.C = C; this.cov = double.NaN; this.x = double.NaN; // estimated signal without noise } /// <summary> /// Constructor /// </summary> /// <param name="R">R Process noise</param> /// <param name="Q">Q Measurement noise</param> public FilterKalman(double R, double Q) { this.R = R; this.Q = Q; } /// <summary> /// Filters a measurement /// </summary> /// <param name="measurement">The measurement value to be filtered</param> /// <param name="u">The controlled input value</param> /// <returns>The filtered value</returns> public double filter(double measurement, double u) { if (double.IsNaN(this.x)) { this.x = (1 / this.C) * measurement; this.cov = (1 / this.C) * this.Q * (1 / this.C); } else { double predX = (this.A * this.x) + (this.B * u); double predCov = ((this.A * this.cov) * this.A) + this.R; // Kalman gain double K = predCov * this.C * (1 / ((this.C * predCov * this.C) + this.Q)); // Correction this.x = predX + K * (measurement - (this.C * predX)); this.cov = predCov - (K * this.C * predCov); } return this.x; } /// <summary> /// Filters a measurement /// </summary> /// <param name="measurement">The measurement value to be filtered</param> /// <returns>The filtered value</returns> public double filter(double measurement) { double u = 0; if (double.IsNaN(this.x)) { this.x = (1 / this.C) * measurement; this.cov = (1 / this.C) * this.Q * (1 / this.C); } else { double predX = (this.A * this.x) + (this.B * u); double predCov = ((this.A * this.cov) * this.A) + this.R; // Kalman gain double K = predCov * this.C * (1 / ((this.C * predCov * this.C) + this.Q)); // Correction this.x = predX + K * (measurement - (this.C * predX)); this.cov = predCov - (K * this.C * predCov); } return this.x; } /// <summary> /// Set the last measurement. /// </summary> /// <returns>return The last measurement fed into the filter</returns> public double lastMeasurement() { return this.x; } /// <summary> /// Sets measurement noise /// </summary> /// <param name="noise">The new measurement noise</param> public void setMeasurementNoise(double noise) { this.Q = noise; } /// <summary> /// Sets process noise /// </summary> /// <param name="noise">The new process noise</param> public void setProcessNoise(double noise) { this.R = noise; } } }
using System; using System.Linq; namespace FusionFiltering { public class ProgramTest { /// <summary> /// kalman滤波测试1 /// </summary> [System.Diagnostics.Conditional("DEBUG")] public static void TestKalmanFilter1() { Console.WriteLine("FilterKalman Usage"); //标准误差 测量误差 FilterKalman test = new FilterKalman(0.001, 20); double[] testData = { 66, 64, 63, 63, 63, 66, 65, 67, 58 ,99,200,65,55,75,85,88,86,89,84,83}; foreach (var x in testData) { Console.WriteLine("Input data: {0:#,##0.00}, Filtered data:{1:#,##0.000}", x, Math.Round(test.filter(x))); } } /// <summary> /// Example Usage with controlled input /// </summary> [System.Diagnostics.Conditional("DEBUG")] public static void TestKalmanFilterWithControlled() { Console.WriteLine("FilterKalman Usage with controlled input"); //标准误差 测量误差 状态向量 控制向量 测量向量 FilterKalman test = new FilterKalman(0.008, 0.1, 1, 1, 1); double[] testData = { 66, 64, 63, 63, 63, 66, 65, 67, 58, 99, 200, 65, 55, 75, 85, 88, 86, 89, 84, 83 }; double u = 0.2; foreach (var x in testData) { Console.WriteLine("Input data: {0:#,##0.00}, Filtered data:{1:#,##0.000}", x,Math.Round( test.filter(x, u))); } } } }
https://github.com/Singerwall/filter/blob/master/filter.c
/** @copyright XunFang Communication Tech Limited. All rights reserved. 2013. * @file filter.c * @author hzp * @version V1.0.0 * @date 12/28/2017 * @brief 旋转编码器的驱动 */ /** * @brief 将32位的主机数据流转换为网络流 * @details Q:过程噪声,Q增大,动态响应变快,收敛稳定性变坏 R:测量噪声,R增大,动态响应变慢,收敛稳定性变好 * @param ResrcData 需要过滤的数据 * @param * @return 过滤之后的数据 */ double KalmanFilter(const double ResrcData,double ProcessNiose_Q,double MeasureNoise_R) { double R = MeasureNoise_R; double Q = ProcessNiose_Q; static double x_last; double x_mid = x_last; double x_now; static double p_last; double p_mid ; double p_now; double kg; x_mid=x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1) p_mid=p_last+Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声 /* * 卡尔曼滤波的五个重要公式 */ kg=p_mid/(p_mid+R); //kg为kalman filter,R 为噪声 x_now=x_mid+kg*(ResrcData-x_mid); //估计出的最优值 p_now=(1-kg)*p_mid; //最优值对应的covariance p_last = p_now; //更新covariance 值 x_last = x_now; //更新系统状态值 return x_now; } /** * @brief 滑动加权滤波算法 * @details * @gram p_buff 采样缓存队列 * @gram value 采样值 * @gram n_sample 采样次数 设定好不能改动 * @retval 无 */ float huadongjiaquan(float *p_buff,float value, int n_sample) { float temp; float sum=0; for(int i=1;i < n_sample; i++) { p_buff[i-1] = p_buff[i]; sum += p_buff[i] * i; } p_buff[n_sample-1] = value; sum += value*n_sample; temp = sum /(((n_sample+1)/2 * n_sample)-1); return temp; }
#ifndef _filter_h_include_ #define _filter_h_include_ #include <stdint.h> double KalmanFilter(const double ResrcData,double ProcessNiose_Q,double MeasureNoise_R) #endif /* _algorithm_h_include_ */
卡尔曼滤波的实质是由量测值重构系统的状态向量。它以“预测—实测—修正”的顺序递推,根据系统的量测值来消除随机干扰,再现系统的状态,或根据系统的量测值从被污染的系统中恢复系统的本来面目。
原文:https://www.cnblogs.com/mrguoguo/p/13857048.html