【源码参考】kalman 滤波调试

tuzhuke

资料包有一个kalman文件夹，里面的程序是从拷贝过来的，没有验证，加上测距效果较好，用均值滤波已经达到我的预期，
kalman滤波一直没有提上日程。
直到前两天一坛友发帖说到这个事，我才有开始琢磨
http://51uwb.cn/forum.php?mod=viewthread&tid=209&extra=page%3D1

进过一番苦苦寻找和调试，终于调通了，特此分享给各位坛友。
先看结果（linux下调试结果）
蓝色数据为原始值，红色是原始值加一个随机噪声，绿色为kalman滤波后结果。

从仿真结果上看，kalman滤波后数据波动变小，更加稳定，更接近真实值。

附件源码使用：
linux gcc编译:gcc -o kalman main.c kalman.c matrix.c -lm
然后执行./kalman即可
*目前与资料包代码不一致，后期更新资料包kalman 滤波算法

狂笑韦恩

其实单纯想要点金钱

狂笑韦恩

一维卡尔曼滤波demo

clc;clear;close all
t = 1 : 1000;
nsig = 5 * sin(0.01 * t) + rand(1, length(t)) + randn(1, length(t)) + 5 * cos(0.05 * t + pi/1.5);
kf = zeros(size(nsig));

x = 0; % state, x(k) = a * x(k-1) + b * u(k)
a = 1; % state transfer matrix
b = 0; % control matrix
h = 1; % observer matrix
p = 0; % estimate cov
q = 0.002; % process cov
r = 0.05; % observer cov
g = 0; % kalman gain

gain =zeros(size(nsig));

for i = 1 : length(nsig)
    x = a * x + b * nsig(i);
    p = a * p * a + q;

    g = p * h /(h * p * h + r);
    x = x + g * (nsig(i) - h * x);
    p = (1 - g * h) * p;

    kf(i) = x;
    gain(i) = g;
end

plot(t, nsig, t, kf, t, gain);
legend("noise", "filtered", "kalman gain");
grid minor;

15093199002

15093199002

如何获得金钱

15093199002

逝去的金乌

多谢分享！！

xxYi

感谢楼主分享

YikZ

tuzhuke 发表于 2020-4-18 21:49
测距实际结果，+-5cm到+-2.5cm，稳定度好很多！

感谢分享！！！！！！！！！！！！

wwwyh

谢谢分享！

wwwyh

谢谢分享！