【C#】【引用加原创】C#实现kalman滤波
最近为了项目,同事让我帮他做一个硬件版的kalman滤波器,实现对设备的kalman滤波,以验证他的理论算法。
犹豫了好几天,用dsp吧,我的kalman滤波算法比较简单,有点大材小用。刚好手里有一块arm调试版,也装了wince系统,就准备在.net环境下编一个kalman滤波器。
虽说学的是导航专业,对kalman滤波应该比较熟悉,可是当时学的就不好,所有学的东西都还给导师了。(导师您不会看到这篇文章吧!看来不能放在首页上!)
于是,只能在网上找一些相关资料。感觉现在变的懒了,总是喜欢在别人的代码上改来改去,不愿意去思考了。算了,反正就这么一次。罪过罪过!
国内的资料对于matlab的算法比较多了,网上随便down,但是基于C#的比较少,还好我的搜索能力不是很差,总算让我找到了相关资料。
引用博客的地址是:http://blog.csdn.net/csdnbao/archive/2009/09/24/4590519.aspx
文章把整个算法都写出来了,我也一起贴出来吧!
using System;
using System.Collections.Generic;
using System.Text;namespace SimTransfer
{public class KalmanFacade{#region inner classclass KalmanFilter{int MP; /* number of measurement vector dimensions */int DP; /* number of state vector dimensions */int CP; /* number of control vector dimensions */public Matrix state_pre; /* predicted state (x'(k)):x(k)=A*x(k-1)+B*u(k) */public Matrix state_post; /* corrected state (x(k)):x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) */public Matrix transition_matrix; /* state transition matrix (A) */public Matrix control_matrix; /* control matrix (B)(it is not used if there is no control)*/public Matrix measurement_matrix; /* measurement matrix (H) */public Matrix process_noise_cov; /* process noise covariance matrix (Q) */public Matrix measurement_noise_cov; /* measurement noise covariance matrix (R) */public Matrix error_cov_pre; /* priori error estimate covariance matrix (P'(k)):P'(k)=A*P(k-1)*At + Q)*/Matrix gain; /* Kalman gain matrix (K(k)):K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)*/Matrix error_cov_post; /* posteriori error estimate covariance matrix (P(k)):P(k)=(I-K(k)*H)*P'(k) */Matrix temp1; /* temporary matrices */Matrix temp2;Matrix temp3;Matrix temp4;Matrix temp5;public KalmanFilter(){MP = 1;DP = 2;CP = 0;state_pre = new Matrix(DP, 1);state_pre.Zero();state_post = new Matrix(DP, 1);state_post.Zero();transition_matrix = new Matrix(DP, DP);transition_matrix.SetIdentity(1.0);transition_matrix[0, 1] = 1;process_noise_cov = new Matrix(DP, DP);process_noise_cov.SetIdentity(1.0);measurement_matrix = new Matrix(MP, DP);measurement_matrix.SetIdentity(1.0);measurement_noise_cov = new Matrix(MP, MP);measurement_noise_cov.SetIdentity(1.0);error_cov_pre = new Matrix(DP, DP);error_cov_post = new Matrix(DP, DP);error_cov_post.SetIdentity(1);gain = new Matrix(DP, MP);if (CP > 0){control_matrix = new Matrix(DP, CP);control_matrix.Zero();}//temp1 = new Matrix(DP, DP);temp2 = new Matrix(MP, DP);temp3 = new Matrix(MP, MP);temp4 = new Matrix(MP, DP);temp5 = new Matrix(MP, 1);}public Matrix Predict(){state_pre = transition_matrix.Multiply(state_post);//if (CP>0)//{// control_matrix//}temp1 = transition_matrix.Multiply(error_cov_post);Matrix at = transition_matrix.Transpose();error_cov_pre = temp1.Multiply(at).Add(process_noise_cov);Matrix result = new Matrix(state_pre);return result;}public Matrix Correct(Matrix measurement){temp2 = measurement_matrix.Multiply(error_cov_pre);Matrix ht = measurement_matrix.Transpose();temp3 = temp2.Multiply(ht).Add(measurement_noise_cov);temp3.InvertSsgj();temp4 = temp3.Multiply(temp2);gain = temp4.Transpose();temp5 = measurement.Subtract(measurement_matrix.Multiply(state_pre));state_post = gain.Multiply(temp5).Add(state_pre);error_cov_post = error_cov_pre.Subtract(gain.Multiply(temp2));Matrix result = new Matrix(state_post);return result;}public Matrix AutoPredict(Matrix measurement){Matrix result = Predict();Correct(measurement);return result;}}#endregionpublic KalmanFacade(int valueItem){if (valueItem<=0){throw new Exception("not enough value items!");}kmfilter = new KalmanFilter[valueItem];Random rand = new Random(1001);for (int i = 0; i < valueItem; i++ ){kmfilter[i] = new KalmanFilter();kmfilter[i].state_post[0, 0] = rand.Next(10);kmfilter[i].state_post[1, 0] = rand.Next(10);//kmfilter[i].process_noise_cov.SetIdentity(1e-5);kmfilter[i].measurement_noise_cov.SetIdentity(1e-1);}}private KalmanFilter[] kmfilter = null; public bool GetValue(double[] inValue, ref double[] outValue){if (inValue.Length != kmfilter.Length || outValue.Length != kmfilter.Length){return false;}Matrix[] measures = new Matrix[kmfilter.Length];for (int i = 0; i < kmfilter.Length; i++ ){measures[i] = new Matrix();measures[i][0, 0] = inValue[i];outValue[i] = kmfilter[i].AutoPredict(measures[i])[0, 0];}return true;}}}//==========test=============SimTransfer.KalmanFacade kalman = new SimTransfer.KalmanFacade(1);Random rand = new Random(1001);System.IO.StreamWriter dataFile = new System.IO.StreamWriter("D:\\test.csv");for (int x = 0; x < 2000; x++ ){double y = 100 * Math.Sin((2.0 * Math.PI / (float)200) * x);double noise = 20 * Math.Sin((40.0 * Math.PI / (float)200) * x) + 40 * (rand.NextDouble() - 0.5);double[] z_k = new double[1];z_k[0] = y + noise;double[] y_k = new double[1];kalman.GetValue(z_k, ref y_k);dataFile.WriteLine(y.ToString() + "," + z_k[0].ToString() + "," + y_k[0].ToString());}dataFile.Close();MessageBox.Show("OK!");
源码是很详细,但是注释比较少,看来我还得把程序翻译一遍!
这两天把注释重新写一下!!!
还有一个就是Matrix的类库。
转载于:https://www.cnblogs.com/MobileBo/archive/2010/09/07/1820831.html
【C#】【引用加原创】C#实现kalman滤波相关推荐
- 开源GNSS软件RTKLIB学习总结(一)MarkDown、VS配置RTKLIB、矩阵、最小二乘和Kalman滤波基本函数
本文从昨晚写到今天上午,比较粗糙,且因为本人对GNSS和RTKLIB掌握的还很少,可能错误比较多,各位可以批判着看. 一.前言 本人是导航工程大二的学生,计划读研读博当老师,可能要和GNSS打几十年的 ...
- GNSS算法进阶(二)- kalman滤波单点定位算法代码实现
状态预测 kalman滤波与前面实现的最小二乘最大的区别就在于,kalman滤波用到了历史状态信息,即进行状态预测. 公式中BU(k)(控制输入)在我们定位模型中用不到. 从(k-1)时刻将状态转移到 ...
- Kalman滤波算法解释与实现
转载,原文地址,请支持:http://www.cnblogs.com/ycwang16/p/5999034.html 认知计算,还要从贝叶斯滤波的基本思想讲起.这一部分,我们先回顾贝叶斯公式的数学基础 ...
- kalman滤波——公式推导
此篇为第③篇,多目标跟踪系列文章: 基础demor入门①②:公式推导③④:深入分析初始化参数的影响⑤: ① Matlab Kalman滤波例子--小球跟踪解析 :matlab官方例子,单目标跟踪.匀速 ...
- RTKLIB软件源码学习(Kalman滤波-矩阵先导)
本文是学习记录关于rib源码中使用的kalman滤波,因为整个定位系统存在误差以及不确定性,需要使用kalman滤波进行预测和平滑,在rtklib中使用的是EKF,即扩展kalman滤波,具体关于ka ...
- ADC采集噪声问题及均方根值滤波与Kalman滤波比较
有一阵子笔者在做一个PT100热电阻的调理电路的时候采用了,使用恒流源的方式测热电阻的阻值.为了采集方便,将0.3mA的电流接入PT100直接把ADC输入端接在了PT100的两端.之后再输出温度的时候 ...
- 理解Kalman滤波的使用
Kalman滤波简介 Kalman滤波是一种线性滤波与预测方法,原文为:A New Approach to Linear Filtering and Prediction Problems.文章推导很 ...
- kalman滤波的解释
文章目录 kalman滤波的解释 背景 信号经过线性变换后的分布 信号的相乘 一维信号 多维信号 贝叶斯估计 状态方程 先验分布 后验分布 总结 kalman滤波的解释 背景 这里的信号都是指服从正态 ...
- OpenCV学习笔记(三十六)——Kalman滤波做运动目标跟踪 OpenCV学习笔记(三十七)——实用函数、系统函数、宏core OpenCV学习笔记(三十八)——显示当前FPS OpenC
OpenCV学习笔记(三十六)--Kalman滤波做运动目标跟踪 kalman滤波大家都很熟悉,其基本思想就是先不考虑输入信号和观测噪声的影响,得到状态变量和输出信号的估计值,再用输出信号的估计误差加 ...
最新文章
- C语言打印1000以内的完数
- mvn打包的POm文件
- QoS是否提供更多带宽?-Vecloud
- HTTP与HTTPS——密码学笔记(九)
- Dubbo 在 K8s 下的思考
- [转]设置 UILabel 和 UITextField 的 Padding 或 Insets
- 计算机视觉与深度学习 | SLAM、视觉里程计、VIO、Net-SLAM、激光SLAM、语义SLAM、数据集( state-of-the-art)
- java例7_在Java 7中处理周数
- 简约至上,美炸天的Vue圆环菜单组件
- jdk jenkins 配置ant_Jenkins流水线实践课程
- 硬盘主引导扇区汇编代码
- 数据结构预算法(六) 数组和矩阵(1)
- CentOS 7.4系统安装配置图解教程
- 前端开发 —— js 常用工具函数(utilities)
- goodbye 2018, hello 2019
- NetAssist网络调试助手,学习网络编程第一步
- 我曾在外包公司呆过,谈一谈外包要不要进?
- python的round函数使用
- c语言中反比例函数定义,初三数学反比例函数的定义知识点
- 用R进行多元线性回归分析建模
热门文章
- React开发(162):React关于 this.props.children 总结
- [css] 怎样把一个div居中?怎样把一个浮动元素居中?怎样把绝对定位的div居中?
- [css] 请你解释下什么是浮动和它的工作原理是什么?同时浮动会引起什么问题?
- 前端学习(2780):创建项目和外观
- 工作200:视频上传和图片编辑功能
- 第四十八期:你太菜了,竟然不知道Code Review?
- 第三十九期:收藏 | 第一次有人把“分布式事务”讲的这么简单明了
- exp/imp和expdp/impdp在功能上的区别:
- centos7搭建apache服务器(亲测可用)
- 怎么使用7zip进行分批压缩_怎么使用钢结构抛丸机对钢结构进行除锈?