文章详情

短信预约-IT技能 免费直播动态提醒

请输入下面的图形验证码

提交验证

短信预约提醒成功

C#实现一阶卡尔曼滤波算法的示例代码

2024-04-02 19:55

关注


//FilterKalman.cs

namespace FusionFiltering
{

    public class FilterKalman
    {
        private double A = 1;
        private double B = 0;
        private double H = 1;

        private double R;
        private double Q;

        private double cov = double.NaN;
        private double x = double.NaN;

        public FilterKalman(double R, double Q, double A, double B, double H)
        {
            this.R = R;  //过程噪声 
            this.Q = Q;  //测量噪声

            this.A = A;  //状态转移矩阵
            this.B = B;  //控制矩阵  u为控制向量
            this.H = H;  //将估计范围与单位转化为与系统变量(或者说测量值)一致的范围与单位

            this.cov = double.NaN;
            this.x = double.NaN; // estimated signal without noise
        }

        public FilterKalman(double R, double Q)
        {
            this.R = R;
            this.Q = Q;
        }

        public double filter(double measurement, double u)
        {
            if (double.IsNaN(this.x)) {
                this.x = (1 / this.H) * measurement;
                this.cov = (1 / this.H) * this.Q * (1 / this.H);
            } else {
                double predX = (this.A * this.x) + (this.B * u);
                double predCov = ((this.A * this.cov) * this.A) + this.Q;

                // Kalman gain
                double K = predCov * this.H * (1 / ((this.H * predCov * this.H) + this.Q));

                // Correction
                this.x = predX + K * (measurement - (this.H * predX));
                this.cov = predCov - (K * this.H * predCov);
            }
            return this.x;
        }

        public double filter(double measurement)
        {
            double u = 0;
            if (double.IsNaN(this.x)) {
                this.x = (1 / this.H) * measurement;
                this.cov = (1 / this.H) * this.Q * (1 / this.H);
            } 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.H * (1 / ((this.H * predCov * this.H) + this.Q));

                // Correction
                this.x = predX + K * (measurement - (this.H * predX));
                this.cov = predCov - (K * this.H * predCov);
            }
            return this.x;
        }

        public double lastMeasurement()
        {
            return this.x;
        }

        public void setMeasurementNoise(double noise)
        {
            this.Q = noise;
        }

        public void setProcessNoise(double noise)
        {
            this.R = noise;
        }
    }
}

//ProgramTestData.cs

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.008, 0.1);
            double[] testData = { 66, 64, 63, 63, 63, 66, 65, 67, 58 };
            foreach (var x in testData) {
                Console.WriteLine("Input data: {0:#,##0.00}, Filtered data:{1:#,##0.000}", x, 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 };
            double u = 0.2;
            foreach (var x in testData) {
                Console.WriteLine("Input data: {0:#,##0.00}, Filtered data:{1:#,##0.000}", x, test.filter(x, u));
            }

        }
    }
}

//Program.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using FusionFiltering;
namespace ConsoleApplication1
{
    class Program
    {
        static void Main(string[] args)
        {

            ProgramTest.TestKalmanFilter1();
            Console.ReadKey();
            Console.WriteLine();
            ProgramTest.TestKalmanFilterWithControlled();
            Console.ReadKey();

        }
}
}

效果:

以上就是C#实现一阶卡尔曼滤波算法的示例代码的详细内容,更多关于C#实现一阶卡尔曼滤波算法的资料请关注编程网其它相关文章!

阅读原文内容投诉

免责声明:

① 本站未注明“稿件来源”的信息均来自网络整理。其文字、图片和音视频稿件的所属权归原作者所有。本站收集整理出于非商业性的教育和科研之目的,并不意味着本站赞同其观点或证实其内容的真实性。仅作为临时的测试数据,供内部测试之用。本站并未授权任何人以任何方式主动获取本站任何信息。

② 本站未注明“稿件来源”的临时测试数据将在测试完成后最终做删除处理。有问题或投稿请发送至: 邮箱/279061341@qq.com QQ/279061341

软考中级精品资料免费领

  • 历年真题答案解析
  • 备考技巧名师总结
  • 高频考点精准押题
  • 2024年上半年信息系统项目管理师第二批次真题及答案解析(完整版)

    难度     813人已做
    查看
  • 【考后总结】2024年5月26日信息系统项目管理师第2批次考情分析

    难度     354人已做
    查看
  • 【考后总结】2024年5月25日信息系统项目管理师第1批次考情分析

    难度     318人已做
    查看
  • 2024年上半年软考高项第一、二批次真题考点汇总(完整版)

    难度     435人已做
    查看
  • 2024年上半年系统架构设计师考试综合知识真题

    难度     224人已做
    查看

相关文章

发现更多好内容

猜你喜欢

AI推送时光机
位置:首页-资讯-后端开发
咦!没有更多了?去看看其它编程学习网 内容吧
首页课程
资料下载
问答资讯