卡爾曼濾波c語言源代碼

1、最佳線性濾波理論起源于40年代美國科學(xué)家Wiener和前蘇聯(lián)科學(xué)家等人的研究工作，后人統(tǒng)稱為維納濾波理論。從理論上說，維納濾波的最大缺點(diǎn)是必須用到無限過去的數(shù)據(jù)，不適用于實(shí)時(shí)處理。為了克服這一缺點(diǎn)，60年代Kalman把狀態(tài)空間模型引入濾波理論，并導(dǎo)出了一套遞推估計(jì)算法，后人稱之為卡爾曼濾波理論。卡爾曼濾波是以最小均方誤差為估計(jì)的最佳準(zhǔn)則，來尋求一套遞推估計(jì)的算法，其基本思想是：采用信號(hào)與噪聲的狀態(tài)空間模型，利用前一時(shí)刻地估計(jì)值和現(xiàn)時(shí)刻的觀測值來更新對(duì)狀態(tài)變量的估計(jì)，求出現(xiàn)時(shí)刻的估計(jì)值。它適合于實(shí)時(shí)處理和計(jì)算機(jī)運(yùn)算?，F(xiàn)設(shè)線性時(shí)變系統(tǒng)的離散狀態(tài)防城和觀測方程為：X(k) = F(k,k-1)&#

2、183;X(k-1)+T(k,k-1)·U(k-1)Y(k) = H(k)·X(k)+N(k)其中X(k)和Y(k)分別是k時(shí)刻的狀態(tài)矢量和觀測矢量F(k,k-1)為狀態(tài)轉(zhuǎn)移矩陣U(k)為k時(shí)刻動(dòng)態(tài)噪聲T(k,k-1)為系統(tǒng)控制矩陣H(k)為k時(shí)刻觀測矩陣N(k)為k時(shí)刻觀測噪聲則卡爾曼濾波的算法流程為：1.預(yù)估計(jì)X(k)= F(k,k-1)·X(k-1) 2.計(jì)算預(yù)估計(jì)協(xié)方差矩陣 C(k)=F(k,k-1)×C(k)×F(k,k-1)'+T(k,k-1)×Q(k)×T(k,k-1)' Q(k) = U(k)

3、×U(k)' 3.計(jì)算卡爾曼增益矩陣 K(k) = C(k)×H(k)'×H(k)×C(k)×H(k)'+R(k)(-1) R(k) = N(k)×N(k)' 4.更新估計(jì) X(k)=X(k)+K(k)×Y(k)-H(k)×X(k) 5.計(jì)算更新后估計(jì)協(xié)防差矩陣 C(k) = I-K(k)×H(k)×C(k)×I-K(k)×H(k)'+K(k)×R(k)×K(k)' 6. X(k+1) = X(k) C(k+

4、1) = C(k)重復(fù)以上步驟 其c語言實(shí)現(xiàn)代碼如下：#include "stdlib.h" #include "rinv.c" int lman(n,m,k,f,q,r,h,y,x,p,g) int n,m,k; double f,q,r,h,y,x,p,g; int i,j,kk,ii,l,jj,js; double *e,*a,*b; e=malloc(m*m*sizeof(double); l=m; if (l<n) l=n; a=malloc(l*l*sizeof(double); b=malloc(l*l*sizeof(double);

5、 for (i=0; i<=n-1; i+) for (j=0; j<=n-1; j+) ii=i*l+j; aii=0.0; for (kk=0; kk<=n-1; kk+) aii=aii+pi*n+kk*fj*n+kk; for (i=0; i<=n-1; i+) for (j=0; j<=n-1; j+) ii=i*n+j; pii=qii; for (kk=0; kk<=n-1; kk+) pii=pii+fi*n+kk*akk*l+j; for (ii=2; ii<=k; ii+) for (i=0; i<=n-1; i+) for

6、(j=0; j<=m-1; j+) jj=i*l+j; ajj=0.0; for (kk=0; kk<=n-1; kk+) ajj=ajj+pi*n+kk*hj*n+kk; for (i=0; i<=m-1; i+) for (j=0; j<=m-1; j+) jj=i*m+j; ejj=rjj; for (kk=0; kk<=n-1; kk+) ejj=ejj+hi*n+kk*akk*l+j; js=rinv(e,m); if (js=0) free(e); free(a); free(b); return(js); for (i=0; i<=n-1; i

7、+) for (j=0; j<=m-1; j+) jj=i*m+j; gjj=0.0; for (kk=0; kk<=m-1; kk+) gjj=gjj+ai*l+kk*ej*m+kk; for (i=0; i<=n-1; i+) jj=(ii-1)*n+i; xjj=0.0; for (j=0; j<=n-1; j+) xjj=xjj+fi*n+j*x(ii-2)*n+j; for (i=0; i<=m-1; i+) jj=i*l; bjj=y(ii-1)*m+i; for (j=0; j<=n-1; j+) bjj=bjj-hi*n+j*x(ii-1)*

8、n+j; for (i=0; i<=n-1; i+) jj=(ii-1)*n+i; for (j=0; j<=m-1; j+) xjj=xjj+gi*m+j*bj*l; if (ii<k) for (i=0; i<=n-1; i+) for (j=0; j<=n-1; j+) jj=i*l+j; ajj=0.0; for (kk=0; kk<=m-1; kk+) ajj=ajj-gi*m+kk*hkk*n+j; if (i=j) ajj=1.0+ajj; for (i=0; i<=n-1; i+) for (j=0; j<=n-1; j+) jj

9、=i*l+j; bjj=0.0; for (kk=0; kk<=n-1; kk+) bjj=bjj+ai*l+kk*pkk*n+j; for (i=0; i<=n-1; i+) for (j=0; j<=n-1; j+) jj=i*l+j; ajj=0.0; for (kk=0; kk<=n-1; kk+) ajj=ajj+bi*l+kk*fj*n+kk; for (i=0; i<=n-1; i+) for (j=0; j<=n-1; j+) jj=i*n+j; pjj=qjj; for (kk=0; kk<=n-1; kk+) pjj=pjj+fi*

10、n+kk*aj*l+kk; free(e); free(a); free(b); return(js); C+實(shí)現(xiàn)代碼如下：=kalman.h=/ kalman.h: interface for the kalman class./#if !defined(AFX_KALMAN_H_ED3D740F_01D2_4616_8B74_8BF57636F2C0_INCLUDED_)#define AFX_KALMAN_H_ED3D740F_01D2_4616_8B74_8BF57636F2C0_INCLUDED_#if _MSC_VER > 1000#pragma once#endif / _

11、MSC_VER > 1000#include <math.h>#include "cv.h" class kalman public: void init_kalman(int x,int xv,int y,int yv); CvKalman* cvkalman; CvMat* state; CvMat* process_noise; CvMat* measurement; const CvMat* prediction; CvPoint2D32f get_predict(float x, float y); kalman(int x=0,int xv=0

12、,int y=0,int yv=0); /virtual kalman();#endif / !defined(AFX_KALMAN_H_ED3D740F_01D2_4616_8B74_8BF57636F2C0_INCLUDED_)=kalman.cpp=#include "kalman.h"#include <stdio.h>/* tester de printer toutes les valeurs des vecteurs*/* tester de changer les matrices du noises */* replace state by c

13、vkalman->state_post ? */CvRandState rng;const double T = 0.1;kalman:kalman(int x,int xv,int y,int yv) cvkalman = cvCreateKalman( 4, 4, 0 ); state = cvCreateMat( 4, 1, CV_32FC1 ); process_noise = cvCreateMat( 4, 1, CV_32FC1 ); measurement = cvCreateMat( 4, 1, CV_32FC1 ); int code = -1; /* create m

14、atrix data */ const float A = 1, T, 0, 0, 0, 1, 0, 0, 0, 0, 1, T, 0, 0, 0, 1 ; const float H = 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 ; const float P = pow(320,2), pow(320,2)/T, 0, 0, pow(320,2)/T, pow(320,2)/pow(T,2), 0, 0, 0, 0, pow(240,2), pow(240,2)/T, 0, 0, pow(240,2)/T, pow(240,2)/pow(

15、T,2) ; const float Q = pow(T,3)/3, pow(T,2)/2, 0, 0, pow(T,2)/2, T, 0, 0, 0, 0, pow(T,3)/3, pow(T,2)/2, 0, 0, pow(T,2)/2, T ; const float R = 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 ; cvRandInit( &rng, 0, 1, -1, CV_RAND_UNI ); cvZero( measurement ); cvRandSetRange( &rng, 0, 0.1, 0 );

16、rng.disttype = CV_RAND_NORMAL; cvRand( &rng, state ); memcpy( cvkalman->transition_matrix->data.fl, A, sizeof(A); memcpy( cvkalman->measurement_matrix->data.fl, H, sizeof(H); memcpy( cvkalman->process_noise_cov->data.fl, Q, sizeof(Q); memcpy( cvkalman->error_cov_post->dat

17、a.fl, P, sizeof(P); memcpy( cvkalman->measurement_noise_cov->data.fl, R, sizeof(R); /cvSetIdentity( cvkalman->process_noise_cov, cvRealScalar(1e-5) ); /cvSetIdentity( cvkalman->error_cov_post, cvRealScalar(1); /cvSetIdentity( cvkalman->measurement_noise_cov, cvRealScalar(1e-1) ); /* c

18、hoose initial state */ state->data.fl0=x; state->data.fl1=xv; state->data.fl2=y; state->data.fl3=yv; cvkalman->state_post->data.fl0=x; cvkalman->state_post->data.fl1=xv; cvkalman->state_post->data.fl2=y; cvkalman->state_post->data.fl3=yv; cvRandSetRange( &rng,

19、 0, sqrt(cvkalman->process_noise_cov->data.fl0), 0 ); cvRand( &rng, process_noise ); CvPoint2D32f kalman:get_predict(float x, float y) /* update state with current position */ state->data.fl0=x; state->data.fl2=y; /* predict point position */ /* x'k=A鈥k+B鈥k P'k=A鈥k-1*AT + Q *

20、/ cvRandSetRange( &rng, 0, sqrt(cvkalman->measurement_noise_cov->data.fl0), 0 ); cvRand( &rng, measurement ); /* xk=A?xk-1+B?uk+wk */ cvMatMulAdd( cvkalman->transition_matrix, state, process_noise, cvkalman->state_post ); /* zk=H?xk+vk */ cvMatMulAdd( cvkalman->measurement_matrix, cvkalman->state_post, measurement, measurement ); /* adjust Kalman filter state */ /* Kk=P'k鈥T鈥?H鈥'k鈥T+R)-1 xk=x'k+Kk鈥?zk-H鈥&#