卡爾曼濾波理論很容易就可以在MATLAB軟體環境下實作,但是,實際的硬體闆子上還是需要C語言,當然可以自動代碼生成,還有一種就是直接手動編寫C語言。
1.前言
在google上搜尋卡爾曼濾波,很容易找到以下這個文章:http://blog.csdn.net/lanbing510/article/details/8828109
文章最後用matlab實作了kalman,然後部落客的前面一些文章也有詳細轉載相關貼子,自己也給出了一些源代碼,例如轉載的這篇卡爾曼濾波器通俗介紹:https://blog.csdn.net/weixin_38451800/article/details/85462129
2.卡爾曼濾波的C語言
網上很多的是關于多元kalman實作。參照網上的一些代碼,本文實作了一個一維地濾波,對于有C語言基礎的同學來講,了解起來應該很容易了。
百度百科裡面有這個文章:http://wenku.baidu.com/view/8523cb6eaf1ffc4ffe47ac24.html
講解的是一維kalman濾波器,但是最後printf出來的都是分立的值,看不出來什麼。參考那段代碼,改寫成了下面這段代碼,在labwindows裡面繪制了一段曲線,效果就很直覺了:
/*-------------------------------------------------------------------------------------------------------------*/
void KalmanFilter(unsigned int ResrcDataCnt,const double *ResrcData,double *FilterOutput,double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction)
{
unsigned int i;
double R = MeasureNoise_R;
double Q = ProcessNiose_Q;
double x_last = *ResrcData;
double x_mid = x_last;
double x_now;
double p_last = InitialPrediction;
double p_mid ;
double p_now;
double kg;
for(i=0;i<ResrcDataCnt;i++)
{
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+i)-x_mid);//估計出的最優值
p_now=(1-kg)*p_mid;//最優值對應的covariance
*(FilterOutput + i) = x_now;
p_last = p_now; //更新covariance值
x_last = x_now; //更新系統狀态值
}
}
/*-------------------------------------------------------------------------------------------------------------*/
參考上面的代碼,優化了一下之後(運作在STM32上):
/*-------------------------------------------------------------------------------------------------------------*/
/*
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*(ResrcData-x_mid);//估計出的最優值
p_now=(1-kg)*p_mid;//最優值對應的covariance
p_last = p_now; //更新covariance值
x_last = x_now; //更新系統狀态值
return x_now;
}
/*-------------------------------------------------------------------------------------------------------------*/
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
3.接下來,是另外一個版本的單維卡爾曼濾波的C語言源代碼:
#include "stdio.h"
#include "stdlib.h"
#include "math.h"
double frand()
{ return 2*((rand()/(double)RAND_MAX) - 0.5); // 随機噪聲}
void main()
{ float x_last=0;
float p_last=0.02;
float Q=0.018;
float R=0.542;
float kg;
float x_mid;
float x_now;
float p_mid;
float p_now;
float z_real=0.56;//0.56
float z_measure;
float sumerror_kalman=0;
float sumerror_measure=0;
int i;
x_last=z_real+frand()*0.03;
x_mid=x_last;
for(i=0;i<20;i++)
{ 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為噪聲
z_measure=z_real+frand()*0.03; // 測量值
x_now=x_mid+kg*(z_measure-x_mid); // 估計出的最優值
p_now=(1-kg)*p_mid; // 最優值對應的covariance
printf("Real position: %6.3f \n",z_real); // 顯示真值
printf("Mesaured position: %6.3f [diff:%.3f]\n",z_measure,fabs(z_real-z_measure));
// 顯示測量值以及真值與測量值之間的誤差
printf("Kalman position: %6.3f [diff:%.3f]\n",x_now,fabs(z_real - x_now)); // 顯示kalman 估計值以及真值和卡爾曼估計值的誤差
sumerror_kalman += fabs(z_real - x_now); //kalman 估計值的累積誤差
sumerror_measure += fabs(z_real-z_measure); // 真值與測量值的累積誤差
p_last = p_now; // 更新covariance 值
x_last = x_now; // 更新系統狀态值
}
printf(" 總體測量誤差 : %f\n",sumerror_measure); // 輸出測量累積誤差
printf(" 總體卡爾曼濾波誤差: %f\n",sumerror_kalman); // 輸出kalman 累積誤差
printf(" 卡爾曼誤差所占比例: %d%% \n",100-(int)((sumerror_kalman/sumerror_measure)*100));
——————————————————————————————————————————
部分内容轉載自:https://www.amobbs.com/thread-5571611-1-1.html 和https://wenku.baidu.com/view/8523cb6eaf1ffc4ffe47ac24.html