1、卡爾曼濾波簡介及其算法實現(xiàn)代碼( C+/C/MATLAB)卡爾曼濾波器簡介 近來發(fā)現(xiàn)有些問題很多人都很感興趣。所以在這里希望能盡自己能力跟大 家討論一些力所能及的算法。現(xiàn)在先討論一下卡爾曼濾波器，如果時間和能力 允許，我還希望能夠?qū)憣懫渌乃惴ǎ邕z傳算法，傅立葉變換，數(shù)字濾波， 神經(jīng)網(wǎng)絡(luò)，圖像處理等等。因為這里不能寫復(fù)雜的數(shù)學(xué)公式，所以也只能形象的描述。希望如果哪位是這 方面的專家，歡迎討論更正。卡爾曼濾波器-Kalma n Filter1什么是卡爾曼濾波器(What is the Kalman Filter? ) 在學(xué)習(xí)卡爾曼濾波器之前，首先看看為什么叫“卡爾曼”。跟其他著名的理論 (例

2、如傅立葉變換，泰勒級數(shù)等等)一樣，卡爾曼也是一個人的名字，而跟他 們不同的是，他是個現(xiàn)代人！卡爾曼全名 Rudolf Emil Kalman ，匈牙利數(shù)學(xué)家， 1930年出生于匈牙利首都布 達佩斯。1953，1954 年于麻省理工學(xué)院分別獲得電機工程學(xué)士及碩士學(xué)位。1957年于哥倫比亞大學(xué)獲得博士學(xué)位。我們現(xiàn)在要學(xué)習(xí)的卡爾曼濾波器，正是源于 他的博士論文和 1960年發(fā)表的論文 A NewApproach to Linear Filtering and Prediction Problems (線性濾波與預(yù)測問題的新方法)。如果對這編論文有 興趣，可以到這里的地址下載： http:/www.c

3、/welch/media/pdf/Kalman1960.pdf 。簡單來說，卡爾曼濾波器是一個“ optimal recursive data processingalgorithm (最優(yōu)化自回歸數(shù)據(jù)處理算法)”。對于解決很大部分的問題，他是 最優(yōu)，效率最高甚至是最有用的。他的廣泛應(yīng)用已經(jīng)超過 30 年，包括機器人導(dǎo) 航，控制，傳感器數(shù)據(jù)融合甚至在軍事方面的雷達系統(tǒng)以及導(dǎo)彈追蹤等等。近 年來更被應(yīng)用于計算機圖像處理，例如頭臉識別，圖像分割，圖像邊緣檢測等 等。2卡爾曼濾波器的介紹(Introduction to the Kalman Filter)為了可以更加容易的理解卡爾

4、曼濾波器，這里會應(yīng)用形象的描述方法來講解， 而不是像大多數(shù)參考書那樣羅列一大堆的數(shù)學(xué)公式和數(shù)學(xué)符號。但是，他的 5 條公式是其核心內(nèi)容。結(jié)合現(xiàn)代的計算機，其實卡爾曼的程序相當(dāng)?shù)暮唵危?要你理解了他的那 5 條公式。在介紹他的 5 條公式之前，先讓我們來根據(jù)下面的例子一步一步的探索。假設(shè)我們要研究的對象是一個房間的溫度。根據(jù)你的經(jīng)驗判斷，這個房間的溫 度是恒定的，也就是下一分鐘的溫度等于現(xiàn)在這一分鐘的溫度(假設(shè)我們用一 分鐘來做時間單位)。假設(shè)你對你的經(jīng)驗不是 100%的相信，可能會有上下偏差 幾度。我們把這些偏差看成是高斯白噪聲( White Gaussian Noise )，也就是 這些偏

5、差跟前后時間是沒有關(guān)系的而且符合高斯分配( Gaussian Distribution )。另外，我們在房間里放一個溫度計，但是這個溫度計也不準(zhǔn) 確的，測量值會比實際值偏差。我們也把這些偏差看成是高斯白噪聲。好了，現(xiàn)在對于某一分鐘我們有兩個有關(guān)于該房間的溫度值：你根據(jù)經(jīng)驗的預(yù) 測值(系統(tǒng)的預(yù)測值)和溫度計的值(測量值)。下面我們要用這兩個值結(jié)合 他們各自的噪聲來估算出房間的實際溫度值。假如我們要估算 k 時刻的是實際溫度值。首先你要根據(jù) k-1 時刻的溫度值，來 預(yù)測 k 時刻的溫度。因為你相信溫度是恒定的，所以你會得到 k 時刻的溫度預(yù) 測值是跟 k-1 時刻一樣的，假設(shè)是 23 度，同時該

6、值的高斯噪聲的偏差是 5度(5 是這樣得到的： 如果 k-1 時刻估算出的最優(yōu)溫度值的偏差是 3，你對自己預(yù)測的 不確定度是 4 度，他們平方相加再開方，就是 5)。然后，你從溫度計那里得到 了 k 時刻的溫度值，假設(shè)是 25 度，同時該值的偏差是 4 度。由于我們用于估算k時刻的實際溫度有兩個溫度值，分別是23度和25度。究竟實際溫度是多少呢？相信自己還是相信溫度計呢？究竟相信誰多一點，我們 可以用他們的covarianee 來判斷。因為 KgA2=5A2/(5A2+4A2)，所以Kg=0.78， 我們可以估算出k時刻的實際溫度值是：23+0.78*(25-23)=24.56 度。可以看 出

7、，因為溫度計的 covariance 比較小(比較相信溫度計)，所以估算出的最優(yōu) 溫度值偏向溫度計的值。現(xiàn)在我們已經(jīng)得到 k 時刻的最優(yōu)溫度值了，下一步就是要進入 k+1 時刻，進行 新的最優(yōu)估算。到現(xiàn)在為止，好像還沒看到什么自回歸的東西出現(xiàn)。對了，在 進入 k+1 時刻之前，我們還要算出 k 時刻那個最優(yōu)值( 24.56 度)的偏差。算 法如下：(1-Kg)*5A2)A0.5=2.35。這里的5就是上面的k時刻你預(yù)測的那個23度溫度值的偏差，得出的 2.35 就是進入 k+1 時刻以后 k 時刻估算出的最優(yōu)溫度 值的偏差(對應(yīng)于上面的 3)。就是這樣，卡爾曼濾波器就不斷的把 covarian

8、ce 遞歸，從而估算出最優(yōu)的溫度 值。他運行的很快，而且它只保留了上一時刻的covarianee。上面的Kg,就是卡爾曼增益(Kalman Gain)。他可以隨不同的時刻而改變他自己的值，是不是 很神奇！ 下面就要言歸正傳，討論真正工程系統(tǒng)上的卡爾曼。3卡爾曼濾波器算法( The Kalman Filter Algorithm )在這一部分，我們就來描述源于 Dr Kalman 的卡爾曼濾波器。下面的描述，會 涉及一些基本的概念知識，包括概率( Probability )，隨即變量( Random Variable )，高斯或正態(tài)分配 (Gaussian Distribution )還有 St

9、ate-space Model 等等。但對于卡爾曼濾波器的詳細證明，這里不能一一描述。首先，我們先要引入一個離散控制過程的系統(tǒng)。該系統(tǒng)可用一個線性隨機微分 方程( Linear Stochastic Difference equation)來描述：X(k)=A X(k-1)+B U(k)+W(k) 再加上系統(tǒng)的測量值：Z(k)=H X(k)+V(k)上兩式子中， X(k) 是 k 時刻的系統(tǒng)狀態(tài)， U(k) 是 k 時刻對系統(tǒng)的控制量。 A 和 B 是系統(tǒng)參數(shù)，對于多模型系統(tǒng)，他們?yōu)榫仃嚒?Z(k) 是 k 時刻的測量值， H 是測 量系統(tǒng)的參數(shù)，對于多測量系統(tǒng)， H 為矩陣。 W(k) 和

10、V(k) 分別表示過程和測量 的噪聲。 他們被假設(shè)成高斯白噪聲 (White Gaussian Noise) ，他們的 covariance 分別是Q R (這里我們假設(shè)他們不隨系統(tǒng)狀態(tài)變化而變化)。對于滿足上面的條件 ( 線性隨機微分系統(tǒng)，過程和測量都是高斯白噪聲 ) ，卡爾 曼濾波器是最優(yōu)的信息處理器。下面我們來用他們結(jié)合他們的 covariances 來 估算系統(tǒng)的最優(yōu)化輸出(類似上一節(jié)那個溫度的例子)。首先我們要利用系統(tǒng)的過程模型，來預(yù)測下一狀態(tài)的系統(tǒng)。假設(shè)現(xiàn)在的系統(tǒng)狀態(tài)是k，根據(jù)系統(tǒng)的模型，可以基于系統(tǒng)的上一狀態(tài)而預(yù)測出現(xiàn)在狀態(tài)：X(k|k-1)=A X(k-1|k-1)+B U(k

11、) 式(1) 中， X(k|k-1) 是利用上一狀態(tài)預(yù)測的結(jié)果， X(k-1|k-1) 是上一狀態(tài)最優(yōu)的 結(jié)果， U(k) 為現(xiàn)在狀態(tài)的控制量，如果沒有控制量，它可以為 0。到現(xiàn)在為止， 我們的系統(tǒng)結(jié)果已經(jīng)更新了， 可是，對應(yīng)于 X(k|k-1) 的 covariance 還沒更新。我們用 P 表示 covariance ：P(k|k-1)=A P(k-1|k-1) A ' +Q (2)式 (2) 中， P(k|k-1) 是 X(k|k-1) 對應(yīng)的 covariance ， P(k-1|k-1) 是 X(k-1|k-1) 對應(yīng)的covarianee , A'表示A的轉(zhuǎn)置矩陣，

12、Q是系統(tǒng)過程的covarianee。式 子 1， 2就是卡爾曼濾波器 5個公式當(dāng)中的前兩個，也就是對系統(tǒng)的預(yù)測。現(xiàn)在我們有了現(xiàn)在狀態(tài)的預(yù)測結(jié)果，然后我們再收集現(xiàn)在狀態(tài)的測量值。結(jié)合 預(yù)測值和測量值，我們可以得到現(xiàn)在狀態(tài) (k) 的最優(yōu)化估算值 X(k|k) ：X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1) (3)其中 Kg 為卡爾曼增益 (Kalman Gain) ：Kg(k)= P(k|k- 1) H ' / (H P(k|k -1) H ' + R) 到現(xiàn)在為止，我們已經(jīng)得到了 k 狀態(tài)下最優(yōu)的估算值 X(k|k) 。但是為了要另卡 爾曼濾波

13、器不斷的運行下去直到系統(tǒng)過程結(jié)束，我們還要更新 k 狀態(tài)下 X(k|k) 的 covariance ：P(k|k)=(I-Kg(k) H ) P(k|k- 1) (5)其中 I 為 1 的矩陣，對于單模型單測量， I=1 。當(dāng)系統(tǒng)進入 k+1 狀態(tài)時， P(k|k)就是式子 (2) 的 P(k-1|k-1) 。這樣，算法就可以自回歸的運算下去 卡爾曼濾波器的原理基本描述了， 式子 1，2，3，4和 5就是他的 5 個基本公式 根據(jù)這 5 個公式，可以很容易的實現(xiàn)計算機的程序。下面，我會用程序舉一個實際運行的例子4簡單例子( A Simple Example ) 這里我們結(jié)合第二第三節(jié)，舉一個非

14、常簡單的例子來說明卡爾曼濾波器的工作 過程。所舉的例子是進一步描述第二節(jié)的例子，而且還會配以程序模擬結(jié)果。根據(jù)第二節(jié)的描述，把房間看成一個系統(tǒng)，然后對這個系統(tǒng)建模。當(dāng)然，我們 見的模型不需要非常地精確。我們所知道的這個房間的溫度是跟前一時刻的溫 度相同的，所以A=1。沒有控制量，所以U(k)=O。因此得出：X(k|k-1)=X(k-1|k-1) (6)式子( 2)可以改成：P(k|k-1)=P(k-1|k-1) +Q (7)因為測量的值是溫度計的，跟溫度直接對應(yīng)，所以H=1。式子3, 4, 5可以改成以下：X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1) (8)Kg(

15、k)= P(k|k-1) / (P(k|k-1) + R) (9)P(k|k)=(1-Kg(k) ) P(k|k- 1) (10)現(xiàn)在我們模擬一組測量值作為輸入。 假設(shè)房間的真實溫度為 25度，我模擬了 2OO 個測量值，這些測量值的平均值為 25度，但是加入了標(biāo)準(zhǔn)偏差為幾度的高斯白 噪聲(在圖中為藍線)。為了令卡爾曼濾波器開始工作，我們需要告訴卡爾曼兩個零時刻的初始值，是X(0|0) 和 P(0|0) 。他們的值不用太在意，隨便給一個就可以了，因為隨著卡爾 曼的工作，X會逐漸的收斂。但是對于P, 般不要取0,因為這樣可能會令卡 爾曼完全相信你給定的 X(0|0) 是系統(tǒng)最優(yōu)的，從而使算法不能

16、收斂。我選了X(0|0)=1 度， P(0|0)=10 。該系統(tǒng)的真實溫度為 25度，圖中用黑線表示。圖中紅線是卡爾曼濾波器輸出的 最優(yōu)化結(jié)果(該結(jié)果在算法中設(shè)置了 Q=1e-6， R=1e-1)。最佳線性濾波理論起源于40年代美國科學(xué)家Wiener和前蘇聯(lián)科學(xué)家K0孔MOrOpOB 等人的研究工作，后人統(tǒng)稱為維納濾波理論。從理論上說, 維納濾波的最大缺點是必須用到無限過去的數(shù)據(jù)，不適用于實時處理。為了克 服這一缺點， 60 年代 Kalman 把狀態(tài)空間模型引入濾波理論，并導(dǎo)出了一套 遞推估計算法，后人稱之為卡爾曼濾波理論。卡爾曼濾波是以最小均方誤差為 估計的最佳準(zhǔn)則，來尋求一套遞推估計的算

17、法，其基本思想是：采用信號與噪 聲的狀態(tài)空間模型，利用前一時刻地估計值和現(xiàn)時刻的觀測值來更新對狀態(tài)變 量的估計，求出現(xiàn)時刻的估計值。它適合于實時處理和計算機運算。現(xiàn)設(shè)線性時變系統(tǒng)的離散狀態(tài)防城和觀測方程為：X(k) = F(k,k-1) X(k-1)+T(k,k- 1) U(k-1)Y(k) = H(k) X(k)+N(k)其中，X(k)和Y(k)分別是k時刻的狀態(tài)矢量和觀測矢量F(k,k-1) 為狀態(tài)轉(zhuǎn)移矩陣U(k) 為 k 時刻動態(tài)噪聲T(k,k-1) 為系統(tǒng)控制矩陣H(k) 為 k 時刻觀測矩陣N(k) 為 k 時刻觀測噪聲則卡爾曼濾波的算法流程為：預(yù)估計 X(k)A= F(k,k- 1

18、) X(k-1)1. 計算預(yù)估計協(xié)方差矩陣C(k)A=F(k,k- 1) x C(k) x F(k,k -1)'+T(k,k-1) x Q(k) x T(k,k -1)'Q(k) = U(k) x U(k)'2. 計算卡爾曼增益矩陣K(k) = C(k)A xH(k)' xH(k) xC(k)A xH(k)'+R(k)A(-1)R(k) = N(k) x N(k)'3. 更新估計X(k)=X(k)A+K(k) xY(k) -H(k)xX(k)A4. 計算更新后估計協(xié)防差矩陣C(k) =I- K(k) x H(k) xC(k)AxI -K(k) x

19、 H(k)'+K(k) xR(k)xK(k)'5. X(k+1) = X(k)C(k+1) = C(k)重復(fù)以上步驟*M atlab實現(xiàn)代碼*% Constant Velocity Model Kalman Filter Simulation % %= clear all; close all; clc;% Initial conditionts = 1; % Sampling timet = 0:ts:100;T = length(t);% Initial statex = 0 40 0 20'x_hat = 0 0 0 0'% Process noise c

20、ovarianceq = 5Q = q*eye(2);% Measurement noise covariancer = 5R = r*eye(2);% Process and measurement noisew = sqrt(Q)*randn(2,T); % Process noisev = sqrt(R)*randn(2,T); % Measurement noise% Estimate error covariance initializationp = 5;P(:,:,1) = p*eye(4);%= % Continuous-time state space model%x_dot

21、(t) = Ax(t)+Bu(t)z(t) = Cx(t)+Dn(t)%A = 0 1 0 0;0 0 0 0;0 0 0 1; 0 0 0 0;B = 0 0;1 0;0 0;0 1;C = 1 0 0 0;0 0 1 0;D = 1 0;0 1;% Discrete-time state space model%x(k+1) = Fx(k)+Gw(k)z(k) = Hx(k)+Iv(k)Continuous to discrete form by zoh% sysc = ss(A,B,C,D);sysd = c2d(sysc, ts, 'zoh');F G H I = ss

22、data(sysd);% Practice state of targetfor i = 1:T-1x(:,i+1) = F*x(:,i);endx = x+G*w; % State variable with noisez = H*x+I*v; % Measurement value with noise%= % Kalman Filterfor i = 1:T-1% Prediction phase x_hat(:,i+1) = F*x_hat(:,i);% State estimate predictP(:,:,i+1) = F*P(:,:,i)*F'+G*Q*G'% T

23、racking error covariance predictP_predicted(:,:,i+1) = P(:,:,i+1);% Kalman gainK = P(:,:,i+1)*H'*inv(H*P(:,:,i+1)*H'+R);% Updata step x_hat(:,i+1)=x_hat(:,i+1)+K*(z(:,i+1)-H*x_hat(:,i+1);% State estimate update P(:,:,i+1)=P(:,:,i+1)-K*H*P(:,:,i+1);% Tracking error covariance update P_updated

24、(:,:,i+1)=P(:,:,i+1);end%=% Estimate error x_error = x-x_hat;% Graph 1 practical and tracking position figure(1) plot(x(1,:),x(3,:),'r');hold on; plot(x_hat(1,:),x_hat(3,:),'g.'); title('2D Target Position') legend('Practical Position','Tracking Position') xla

25、bel('X axis m') ylabel('Y axis m') hold off;% Graph 2 figure(2) plot(t,x(1,:),grid on; hold on;plot(t,x_hat(1,:),'r'),grid on;title('Practical and Tracking Position on X axis') legend('Practical Position','Tracking Position') xlabel('Time sec')

26、 ylabel('Position m') hold off;% Graph 3 figure(3) plot(t,x_error(1,:),grid on; title('Position Error on X axis') xlabel('Time sec') ylabel('Position RMSE m') hold off;% Graph 4 figure(4) plot(t,x(2,:),grid on; hold on;plot(t,x_hat(2,:),'r'),grid on;title('

27、;Practical and Tracking Velocity on X axis') legend('Practical Velocity','Tracking Velocity') xlabel('Time sec') ylabel('Velocity m/sec') hold off;% Graph 5 figure(5) plot(t,x_error(2,:),grid on; title('Velocity Error on X axis') xlabel('Time sec')

28、 ylabel('Velocity RMSE m/sec') hold off;*c 語言實現(xiàn)代碼 轉(zhuǎ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=mal

29、loc(l*l*sizeof(double);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&l

30、t;=n-1; i+)for (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&l

31、t;=n-1; i+)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-

32、1)*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=i*l+

33、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*n+kk*aj*l+kk

34、; free(e); free(a); free(b); return(js);*C+實現(xiàn)代碼轉(zhuǎn) / kalman.h: interface for the kalman class.#if !defined(AFX_KALMAN_H_ED3D740F_01D2_4616_8B74_8BF57636F2C0_INCL UDED_)#define AFX_KALMAN_H_ED3D740F_01D2_4616_8B74_8BF57636F2C0_INCLUDED_ #if _MSC_VER > 1000 #pragma once#endif / _MSC_VER > 1000#inc

35、lude <math.h>#include "cv.h" class kalmanpublic: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,int y=0,int yv=0); /virtual

36、 kalman();#endif=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 cvkalman->state_post ? */ CvRandState rng;const double T = 0.1;kalman:kalman(int x,int xv,int y,

37、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 matrix 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,

38、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(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,

39、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 ); rng.disttype = CV_RAND_NORMAL;cvRand( &rng, state );memcpy( cvkalman->transition_matrix->data.fl, A, sizeof

40、(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->data.fl, P, sizeof(P);memcpy( cvkalman->measurement_noise_cov->data.fl, R, sizeof(R);/* choose initial state */ state->data.fl0=x;state->data.fl1=xv;state->data.fl2=y;state->data.fl3=yv; cvkalman->state_post->