卡尔曼滤波器代码,CSharp代码库,德仔网

代码语言

代码分类

【CSharp】卡尔曼滤波器

作者:DDT / 发布于2012/7/23/ 456

卡尔曼滤波器

<div>using System;

namespace NemoTrack
{
/// <summary>
/// Kalman 的摘要说明。
/// </summary>
public class Kalman
{
private double R, Q;
private double currentX, nextX;
private double currentP, nextP;
private double K;
private double rootR;
private double d, d_, currentsample;
private double alpha = 0.8;

public Kalman(double x0, double R0, double Q0)
{
//
// TODO: 在此处添加构造函数逻辑
//
currentX = x0;
currentsample = x0;
nextX = x0;
R = R0;
Q = Q0;
currentP = 1.0;
d = 0;
d_ = 0;
rootR = Math.Sqrt(R);
}

public double Next(double sample)
{
currentX = nextX;
currentP = nextP + Q;
d_ = d;

K = currentP / (currentP + R);
d = d_ + K * (sample - currentsample - d_);
nextX = currentX + K * (sample - currentX) + alpha * d;
nextP = (1 - K) * currentP;

currentsample = sample;

if (nextX > 1000)
{
int j = 1;
}

return nextX;
}




}
}
</div>

评论列表

本站所提供的代码，版权归原作者所有，若有侵犯作者版权，请与我们联系，我们将立即删除或修改。谢谢!
本站所有代码发布及提供者。

试试其它关键字

　卡尔曼滤波器　

同语言下

可能有用的

DDT贡献的其它代码(160)