卡尔曼的C语言算法对吗?网上搜到的算法都很多是这样

[复制链接]

558|0

手机看帖

扫描二维码
随时随地手机跟帖

电梯直达

楼主

ljxh401|

楼主 | 2021-5-14 01:12 | 只看该作者回帖奖励

|倒序浏览 |阅读模式

C语言, ST, BLE, double, 噪声, CD

代码里面 KG 的卡尔曼增益好像和输入数据没有关系,这样不能通过数据修正增益,那么好像效果,不符合卡尔曼滤波的思想吧
p_mid=p_last+Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
kg=p_mid/(p_mid+R); //kg为kalman filter，R为噪声

kg 和 ResrcData 产生不了关系

复制

/*-------------------------------------------------------------------------------------------------------------*/

/*        

        Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏

        R:测量噪声，R增大，动态响应变慢，收敛稳定性变好        

*/



double KalmanFilter(const double ResrcData,double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction)

{

        double R = MeasureNoise_R;

        double Q = ProcessNiose_Q;



        static        double x_last;



        double x_mid = x_last;

        double x_now;



        static        double p_last;



        double p_mid ;

        double p_now;

        double kg;        



        x_mid=x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)

        p_mid=p_last+Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声

        kg=p_mid/(p_mid+R); //kg为kalman filter，R为噪声

        x_now=x_mid+kg*(<font color="#ff0000">ResrcData</font>-x_mid);//估计出的最优值

                

        p_now=(1-kg)*p_mid;//最优值对应的covariance        



        p_last = p_now; //更新covariance值

        x_last = x_now; //更新系统状态值



        return x_now;                

}



/*-------------------------------------------------------------------------------------------------------------*/