【源码参考】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 滤波算法

tuzhuke

测距实际结果，+-5cm到+-2.5cm，稳定度好很多！


使用方法
测距之前做初始化

1. KalMan_Init();

复制代码

算出距离后，直接传入，下面是原始数据和kalman数据全部打印出来

1. distance = distance - dwt_getrangebias(config.chan,(float)distance, config.prf);
   
2. printf("0x%04X <--> 0x%02X%02X : %.2f  %.2f cm\n",SHORT_ADDR,msg_f_send.destAddr[1],msg_f_send.destAddr[0],distance,KalMan(distance));
   

复制代码

部分原始数据

1. 0x0002 <--> 0x0001 : 1.75  1.73 cm
   
2. 0x0002 <--> 0x0001 : 1.72  1.73 cm
   
3. 0x0002 <--> 0x0001 : 1.70  1.73 cm
   
4. 0x0002 <--> 0x0001 : 1.73  1.73 cm
   
5. 0x0002 <--> 0x0001 : 1.71  1.73 cm
   
6. 0x0002 <--> 0x0001 : 1.77  1.73 cm
   
7. 0x0002 <--> 0x0001 : 1.77  1.74 cm
   
8. 0x0002 <--> 0x0001 : 1.74  1.74 cm
   
9. 0x0002 <--> 0x0001 : 1.72  1.74 cm
   
10. 0x0002 <--> 0x0001 : 1.72  1.74 cm
    
11. 0x0002 <--> 0x0001 : 1.75  1.74 cm
    
12. 0x0002 <--> 0x0001 : 1.69  1.73 cm
    
13. 0x0002 <--> 0x0001 : 1.72  1.73 cm
    
14. 0x0002 <--> 0x0001 : 1.80  1.74 cm
    
15. 0x0002 <--> 0x0001 : 1.77  1.75 cm
    
16. 0x0002 <--> 0x0001 : 1.73  1.74 cm
    
17. 0x0002 <--> 0x0001 : 1.72  1.74 cm
    
18. 0x0002 <--> 0x0001 : 1.74  1.74 cm
    
19. 0x0002 <--> 0x0001 : 1.72  1.74 cm
    
20. 0x0002 <--> 0x0001 : 1.70  1.73 cm
    
21. 0x0002 <--> 0x0001 : 1.71  1.73 cm
    
22. 0x0002 <--> 0x0001 : 1.76  1.74 cm
    
23. 0x0002 <--> 0x0001 : 1.70  1.73 cm
    
24. 0x0002 <--> 0x0001 : 1.70  1.73 cm
    
25. 0x0002 <--> 0x0001 : 1.68  1.72 cm
    
26. 0x0002 <--> 0x0001 : 1.71  1.72 cm
    
27. 0x0002 <--> 0x0001 : 1.74  1.72 cm
    
28. 0x0002 <--> 0x0001 : 1.69  1.72 cm
    
29. 0x0002 <--> 0x0001 : 1.72  1.72 cm
    
30. 0x0002 <--> 0x0001 : 1.70  1.72 cm
    
31. 0x0002 <--> 0x0001 : 1.74  1.72 cm
    
32. 0x0002 <--> 0x0001 : 1.70  1.72 cm
    
33. 0x0002 <--> 0x0001 : 1.76  1.73 cm
    

复制代码