非常感謝原作者，我在這個的基礎上轉換成純整形運算。STM32F103 12位ADC先放大1000倍再運算，理論上可以保留小數點后三位的結果。

效果非常不錯，運算速度也快，72M時鐘 1-2uS左右(根據MDK周期數)。]uint32_t?KalmanFilter(int32_t?ResrcData)

{

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

/*

Q:過程噪聲，Q增大，動態響應變快，收斂穩定性變壞

R:測量噪聲，R增大，動態響應變慢，收斂穩定性變好

*/

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

static?int32_t?R?=?(int32_t)(128*1024);

static?int32_t?Q?=?(int32_t)4;

static?uint32_t?Counter1?=?0;

static?uint32_t?Counter2?=?0;

static?int32_t?x_last?=?0;

static?int32_t?p_last;???//?應賦初始估計值

int32_t?x_mid;

int32_t?x_now;

int32_t?p_mid?;

int32_t?p_now;

ResrcData?*=?1024;

x_now?=?ResrcData?-?x_last;

if(x_now?

{

x_now?*=?-1;?//?取絕對值

}

if(x_now?>=?32*1024)???//?如果測量值連續比估計值大或小?相信測量值，加速迭代

{

Counter1++;

Counter2?=?0;

if(Counter1?>?10)

{

R?=?512;;

Q?=?128;

}

}

else?????????????????//?數據比較穩定，加強濾波

{

Counter1?=?0;

Counter2++;

if(Counter2?>?10)

{

R?=?(int32_t)(128*1024);

Q?=?(int32_t)4;

}

}

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*(ResrcData?-?x_mid);//?估計出的最優值

x_now?=?x_mid?+?(p_mid*(ResrcData?-?x_mid))/(p_mid?+?R);

//????p_now?=?(1?-?kg)*p_mid;?//?最優值對應的covariance

p_now?=?p_mid?-?p_mid*p_mid/(p_mid?+?R);?//?最優值對應的covariance

p_last?=?p_now;??//?更新covariance值

x_last?=?x_now;??//?更新系統狀態值

x_now?/=?1024;

if((x_now?>?4096)||(?x_now?

{

x_last?=?ResrcData;

p_now?=?ResrcData;

x_now?=?ResrcData/1024;

}

return?(u32)x_now;

}

總結

以上是生活随笔為你收集整理的算法转换c语言程序,(转)C语言实现卡尔曼滤波算法程序的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。