卡尔曼滤波的C函数？？

caicaptain2

最近研究了一下卡尔曼滤波，参考了网上的几个文章，比如这个https://www.cnblogs.com/guiguzhixing/p/5930003.html

想实现一个简单的单个测量值的卡尔曼滤波。 根据这个文章的做法，发现了一些问题。

1. //标量卡尔曼滤波
   
2. typedef struct {   
   
3.   float x;  // 系统的状态量   
   
4.   float A;  // x(n)=A*x(n-1)+u(n),u(n)~N(0,q) 常规实现中，固定为1   
   
5.   float H;  // z(n)=H*x(n)+w(n),w(n)~N(0,r) 常规实现中，固定为1   
   
6.   float q;  // 预测过程噪声协方差   
   
7.   float r;  // 测量过程噪声协方差   
   
8.   float p;  // 估计误差协方差   
   
9.   float gain;//卡尔曼增益
   
10. }  KalmanStructTypedef;
    
11. /**
    
12. *@function: - 卡尔曼滤波器
    
13. *@kalmanFilter:卡尔曼结构体
    
14. *@newMeasured；测量值
    
15. *返回滤波后的值 */
    
16. float kalmanFilter_filter(KalmanStructTypedef *kalmanFilter, float newMeasured)
    
17. {
    
18.     /* Predict */   
    
19. kalmanFilter->x = kalmanFilter->x;  //%x的先验估计由上一个时间点的后验估计值和输入信息给出   
    
20. kalmanFilter->p = kalmanFilter->p + kalmanFilter->q;  /*计算先验均方差 p(n|n-1)=A^2*p(n-1|n-1)+q */   
    
21. /* Correct */
    
22.    kalmanFilter->gain = kalmanFilter->p / (kalmanFilter->p + kalmanFilter->r);   
    
23.    kalmanFilter->x = kalmanFilter->x + kalmanFilter->gain * (newMeasured - kalmanFilter->x); //利用残余的信息改善对x(t)的估计，给出后验估计，这个值也就是输出   
    
24.    kalmanFilter->p = (1 - kalmanFilter->gain) * kalmanFilter->p; //%计算后验均方差
    
25.    return kalmanFilter->x;
    
26. }   

复制代码

这个函数运行时，使用固定的预测方差q和测量方差r，经过十几次滤波后，卡尔曼增益会成为固定的值，从而失去了滤波效果。 卡尔曼增益收敛为固定值后，比如0.01，意义就是，每次测量值的偏差都被强制缩小100倍，那么，很难根据测量数据变化的快慢大小，来动态反应输出结果。
所以，这应该是错误的卡尔曼滤波。

那么，哪位能提供一个卡尔曼滤波函数参考一下？

eric2013

Github，很多，可以测测看那个效果好点。

caicaptain2

eric2013 发表于 2022-3-11 10:13
Github，很多，可以测测看那个效果好点。

看了几个，好像就是这样的效果。 可能是因为简化后的原因，丢失了各种控制变量，预估变量造成的。

ssssssss

caicaptain2 发表于 2022-3-11 12:22
看了几个，好像就是这样的效果。 可能是因为简化后的原因，丢失了各种控制变量，预估变量造成的。

有没有找到合适的算法没？来一个点处理一个点，把波形变得平滑一些，可以用一阶卡尔曼滤波吗？

caicaptain2

ssssssss 发表于 2022-3-21 08:37
有没有找到合适的算法没？来一个点处理一个点，把波形变得平滑一些，可以用一阶卡尔曼滤波吗？

问题应该出在这个卡尔曼是简化版本，导致很多修正预测的丢失。
后来找了一个平滑滤波的算法，
https://www.armbbs.cn/forum.php? ... 1378&extra=page%3D1

效果待观察