卡爾曼濾波示例

原創 无比机智的永哥 2019-03-21 06:41

在上一篇卡爾曼濾波詳解已經給出詳細的推導過程。這裏給出幾個示例加深理解，這個是幾年前學習卡爾曼濾波時候整理的，比較囉嗦，初學者可以看看，瞭解過的就不用浪費時間了，主要是最近在整理資料差點弄丟，還是放到網上保存吧。

簡單示例1

這個示例中假設狀態是穩定的，也就是真實的狀態是不隨着時間變化的，那麼利用卡爾曼濾波算法最後會收斂到最優的狀態。

參數設置

對於所有的變量，本場景下都是一個具體的數值而不是矩陣：

uu 是被忽略的；

A=1A = 1 因爲已經知道在該場景下，上一個狀態和當前狀態應該是相同的；

H=1H = 1 一般來說測量值都是狀態加上一些噪聲，測量和要估計的狀態是一致的，在實際應用都一般都是1；

x^0=0\hat{x}_0 = 0 設置最初的狀態爲0，肯定不是真實的狀態，但最後系統會收斂到真實狀態；

P0=1P_0 = 1 上面提到過不能設置爲0，可以試一下，最後所有估計的狀態都是0了；

Q=0Q = 0 和 R=0.1R = 0.1 。

基本模型
xk=Axk−1+Buk−1+wk=xk−1 \begin{aligned} x_k & = Ax_{k-1} + Bu_{k-1} + w_k \\ & = x_{k-1} \end{aligned}

zk=Hxk+vk=xk+vk \begin{aligned} z_k & = Hx_k + v_k \\ & = x_k + v_k \end{aligned}

基本公式

Time Update Measurement Update
x^kˉ=x^k−1\bar{\hat{x}_k} = \hat{x}_{k-1} Kk=Pkˉ(Pkˉ+0.1)−1K_k = \bar{P_k}(\bar{P_k} + 0.1)^{-1}
Pkˉ=Pk−1\bar{P_k} = P_{k-1} x^k=x^kˉ+Kk(zk−x^kˉ)\hat{x}_k = \bar{\hat{x}_k} + K_k(z_k - \bar{\hat{x}_k})
Pk=(1−Kk)PkˉP_k = (1 - K_k)\bar{P_k}

仿真實驗

給定測量值：

TIME(ms) 1 2 3 4 5 6 7 8 9 10
VALUE (V) 0.39 0.50 0.48 0.29 0.25 0.32 0.34 0.48 0.41 0.45

基本的卡爾曼濾波算法的代碼：

z = [0, 0.39, 0.50, 0.48, 0.29, 0.25, 0.32, 0.34, 0.48, 0.41, 0.45];
s = size(z);
xp = [0]; Pp = [1]; xpp = [0]; Ppp = [1]; K = [0];
A = 1; H = 1; Q = 0; R = 0.1;
for k=2:s(2)
    xp(k) = A*xpp(k-1);
    Pp(k) = A*Ppp(k-1)*A' + Q;
    K(k) = Pp(k)*H/(H*Pp(k)*H + R);
    xpp(k) = xp(k) + K(k)*(z(k) - H*xp(k));
    Ppp(k) = (1 - K(k)*H)*Pp(k);
end

plot(1:s(2)-1, xpp(2:s(2)), 'r', 'LineWidth', 2);
axis([1 s(2)-1 0 0.5]);

下面是卡爾曼濾波算法運行過程中基本公式變量的變化：

kk zkz_k x^kˉ\bar{\hat{x}_k} Pkˉ\bar{P_k} KkK_k x^k\hat{x}_k PkP_k
0 0 1
1 0.39 0 1 0.9091 0.3545 0.0909
2 0.50 0.3545 0.0909 0.4762 0.4238 0.0476
3 0.48 0.4238 0.0476 0.3226 0.4419 0.0323
4 0.29 0.4419 0.0323 0.2439 0.4049 0.0244
5 0.25 0.4049 0.0244 0.1961 0.3745 0.0196
6 0.32 0.3745 0.0196 0.1639 0.3656 0.0164
7 0.34 0.3656 0.0164 0.1408 0.3620 0.0141
8 0.48 0.3620 0.0141 0.1235 0.3765 0.0123
9 0.41 0.3765 0.0123 0.1099 0.3802 0.0110
10 0.45 0.3802 0.0110 0.0990 0.3871 0.0099

估計狀態的效果圖，可以看出來已經收斂了：

example result2

簡單示例2

在本例子中，估計一個隨機的標量常量，例如電壓，真實的狀態是不隨着時間變化的，那麼利用卡爾曼濾波算法最後會收斂到最優的狀態。

參數設置

對於所有的變量，本場景下都是一個具體的數值而不是矩陣：

uu 是被忽略的；

A=1A = 1 因爲已經知道在該場景下， 上一時刻電壓和當前電壓應該是相同的（不考慮噪聲的話）；

H=1H = 1 電壓表測量的值和要估計的值是相同的（不算噪聲的話）；

x^0=0\hat{x}_0 = 0 設置最初的狀態爲0，肯定不是真實的狀態，但最後系統會收斂到真實狀態；

P0=1P_0 = 1 上面提到過不能設置爲0，可以試一下，最後所有估計的狀態都是0了，只要設置不爲0結果就可以正確收斂；

Q=1e−6Q = 1e-6 和在這裏也可以設置爲0，但設置一個非0小數值可以更靈活的來調整濾波，其實在該示例中生成的數據是沒有過程噪聲的；

R=0.01R = 0.01 電壓表模擬信號轉化爲數字信號不是完全精確有誤差，假設有0.1的RMS白測量噪聲，也就是標準差是0.1，那麼方差爲0.01。

基本模型
xk=Axk−1+Buk−1+wk=xk−1+wk \begin{aligned} x_k & = Ax_{k-1} + Bu_{k-1} + w_k \\ & = x_{k-1} + w_k \end{aligned}

zk=Hxk+vk=xk+vk \begin{aligned} z_k & = Hx_k + v_k \\ & = x_k + v_k \end{aligned}

基本公式

Time Update Measurement Update
x^kˉ=x^k−1\bar{\hat{x}_k} = \hat{x}_{k-1} Kk=Pkˉ(Pkˉ+0.01)−1K_k = \bar{P_k}(\bar{P_k} + 0.01)^{-1}
Pkˉ=Pk−1+(1e−6)\bar{P_k} = P_{k-1} + (1e-6) x^k=x^kˉ+Kk(zk−x^kˉ)\hat{x}_k = \bar{\hat{x}_k} + K_k(z_k - \bar{\hat{x}_k})
Pk=(1−Kk)PkˉP_k = (1 - K_k)\bar{P_k}

仿真實驗

在這裏，設置要獲得的標量常量 x=−0.37727x=-0.37727 。然後去模擬生成50個單獨的測量數據 zkz_k ，對每一個測量值隨機生成測量噪聲（服從均值爲0，標準差爲0.1的高斯分佈）添加到上面設置的常量上生成測量值。同時可能需要做多組實驗，預先生成測量集合可以保持每組實驗數據是完全相同的。

生成數據代碼如下，把生成的數據保存下來，後面運行卡爾曼濾波算法時不再重新生成，直接加載：

x = -0.37727;
num = 50;
mnoise = normrnd(0, 0.1, num, 1);
z = mnoise + x;
save('z.mat', 'z');

plot(1:num, repmat(x, [1, num]), 'k', 'LineWidth', 2);
hold on;
plot(1:num, z, 'b*');
maxz = max(z);
minz = min(z);
axis([0 num+1 minz-0.01 maxz+0.01]);
xlabel('Iteration');  
ylabel('Voltage');  
set(gca,'XTick',10:10:50)  
set(gca,'YTick',-0.6:0.1:-0.1)

生成圖如下，其中黑色線表示真實的狀態值，而藍色*表示添加白色高斯噪聲之後的生成的測量值：

example result3

基本的卡爾曼濾波算法代碼：

x = -0.37727;
num = 50;
mat = load('z.mat');
z = mat.z;

xp = [0]; Pp = [1]; xpp = [0]; Ppp = [1]; K = [0];
A = 1; H = 1; Q = 0.000001; R = 0.01;
for k=2:num+1
    xp(k) = A*xpp(k-1);
    Pp(k) = A*Ppp(k-1)*A' + Q;
    K(k) = Pp(k)*H/(H*Pp(k)*H + R);
    xpp(k) = xp(k) + K(k)*(z(k-1) - H*xp(k));
    Ppp(k) = (1 - K(k)*H)*Pp(k);
end

plot(1:num, repmat(x, [1, num]), 'k', 'LineWidth', 2);
hold on;
plot(1:num, z, 'b*');
plot(1:num, xpp(2:num+1), 'r', 'LineWidth', 2);

maxz = max(z);
minz = min(z);
axis([0 num+1 minz-0.01 maxz+0.01]);
xlabel('Iteration');  
ylabel('Voltage');  
set(gca,'XTick',10:10:50)  
set(gca,'YTick',-0.6:0.1:-0.1)  

% plot P versus iteration
figure;
plot(1:num, Ppp(2:num+1), 'k', 'LineWidth', 2);
xlabel('Iteration');  
ylabel('Pk');  
set(gca,'XTick',10:10:50)  
set(gca,'YTick',0.002:0.002:0.1)

結果如下，可以看出來雖然測量值的噪聲比較大，但是利用卡爾曼濾波算法還是很快收斂到正確的值，紅色線是卡爾曼濾波算法估計的值：

example result4

然後上面考慮到對於 P0P_0 的設置問題，提到只要設置爲不爲0的數值即可，最後結果也會正確的收斂。在下圖中是 PkP_k 的變化曲線，可以看出最後變爲0.0002。

example result5

前面也提到過對於 QQ 和 RR 取值來說不是特別重要，最後結果都會收斂，但也說到，如果對這些噪聲估計的更好，能得到更好的結果。

下面是把上面的 RR 設置爲1得到的結果，該值變大了，說明測量誤差較大，在估計過程中不會太相信這個測量值，所以最開始從初始值向下的波動，而沒有太大的向測量值的波動：

example result6

下面是把上面的 RR 設置爲0.0001得到的結果，該值變小了，說明測量誤差較小，在估計過程中就會儘可能去相信這個測量值（包含噪聲的測量值），所以會有偏向測量值的波動：

example result7

把這三個畫在一張圖上近距離觀察一下，可以更好感受這個變化：

example result8

簡單示例3

在本例子中，估計一個隨機的標量變量，也就是說每個時間的狀態是不一樣的，例如飛機着陸時的海拔（當然也可以估計速度和燃料），那麼利用卡爾曼濾波算法最後會估計每個時刻最優的狀態。

參數設置

對於所有的變量，本場景下都是一個具體的數值而不是矩陣：

uu 是被忽略的；

A=0.98A = 0.98 在該場景下，假設每個時刻飛機海拔下降2%，所以當前時刻飛機海拔是上一時刻飛機海拔的0.98（不考慮噪聲的話）；

H=1H = 1 GPS或氣壓計等傳感器獲得的海拔和真實海拔是一致的（不算噪聲的話）；

x^0=1000\hat{x}_0 = 1000 設置最初的狀態爲1000，最初的飛機海拔；

P0=1P_0 = 1 上面提到過不能設置爲0，可以試一下，最後所有估計的狀態都是0了，只要設置不爲0結果就可以正確收斂；

Q=0Q = 0 在該示例中生成的數據是沒有過程噪聲的；

R=900R = 900 測量海拔的傳感器提供不同程度的精度，標準差是30，那麼方差爲900，其實不用這是這麼高，主要是防止噪聲不明顯看不出來效果。

基本模型
xk=Axk−1+Buk−1+wk=0.98∗xk−1 \begin{aligned} x_k & = Ax_{k-1} + Bu_{k-1} + w_k \\ & = 0.98*x_{k-1} \end{aligned}

zk=Hxk+vk=xk+vk \begin{aligned} z_k & = Hx_k + v_k \\ & = x_k + v_k \end{aligned}

基本公式

Time Update Measurement Update
x^kˉ=0.98∗x^k−1\bar{\hat{x}_k} = 0.98 * \hat{x}_{k-1} Kk=Pkˉ(Pkˉ+900)−1K_k = \bar{P_k}(\bar{P_k} + 900)^{-1}
Pkˉ=0.98∗Pk−1∗0.98\bar{P_k} = 0.98 * P_{k-1} *0.98 x^k=x^kˉ+Kk(zk−x^kˉ)\hat{x}_k = \bar{\hat{x}_k} + K_k(z_k - \bar{\hat{x}_k})
Pk=(1−Kk)PkˉP_k = (1 - K_k)\bar{P_k}

仿真實驗

在這裏，設置最初的飛機海拔爲1000 ，其他時刻的真實海拔爲上一時刻海拔的0.98。然後去模擬生成100個（再多的話沒意義，都是0了）單獨的測量數據 zkz_k ，對每一個測量值隨機生成測量噪聲添加到該時刻的真實海拔上生成測量值。同時可能需要做多組實驗，預先生成測量集合可以保持每組實驗數據是完全相同的。

生成數據代碼如下，把生成的數據保存下來，生成具有隨機性，這裏結果的數據見附件altitude_z1.mat，後面運行卡爾曼濾波算法時不再重新生成，直接加載：

x0 = 1000;
num = 100;
x = [];
for i=1:num
    x0 =  x0 * 0.98;
    x(i) = x0;
end
mnoise = normrnd(0, 30, num, 1);
z = mnoise + x';
save('z.mat', 'z');

plot(1:num, z, 'g', 'LineWidth', 2);
hold on;
plot(1:num, x, 'k', 'LineWidth', 2);

數據分佈圖如下，其中黑色線表示真實的狀態值，而綠色添加白色高斯噪聲之後的生成的測量值：

example result9

基本的卡爾曼濾波算法代碼：

x0 = 1000;
num = 100;
x = [];
for i=1:num
    x0 =  x0 * 0.98;
    x(i) = x0;
end
mat = load('z.mat');
z = mat.z;

xp = [1000]; Pp = [1]; xpp = [1000]; Ppp = [1]; K = [0];
A = 0.98; H = 1; Q = 0; R = 900;
for k=2:num+1
    xp(k) = A*xpp(k-1);
    Pp(k) = A*Ppp(k-1)*A' + Q;
    K(k) = Pp(k)*H/(H*Pp(k)*H + R);
    xpp(k) = xp(k) + K(k)*(z(k-1) - H*xp(k));
    Ppp(k) = (1 - K(k)*H)*Pp(k);
end

plot(1:num, z, 'g', 'LineWidth', 2);
hold on;
plot(1:num, x, 'k', 'LineWidth', 2);
plot(1:num, xpp(2:num+1), 'r', 'LineWidth', 2);
xlabel('Iteration');  
ylabel('Altitude');

結果如下，可以看出來雖然測量值的噪聲比較大，但是利用卡爾曼濾波算法還是很快收斂到正確的值，黑色線是真實的飛機海拔，紅色線是卡爾曼濾波算法估計的海拔，可以看到黑色線幾乎被全部遮擋住了，說明完美擬合了：

example result10

但是爲什麼效果會這麼好呢，好的有點不符合預期，對上面的實驗修改一下：

生成成新的數據但是添加測量噪聲時是在當前時刻的真實飛機海拔上添加[-100, 100]內的一個隨機數，不是高斯噪聲了，增加噪聲來看擬合效果，其他變量條件不變：

生成數據代碼如下，生成具有隨機性，這裏結果的數據見附件altitude_z2.mat：

x0 = 1000;
num = 100;
x = [];
z= [];
for i=1:num
    x0 =  x0 * 0.98;
    x(i) = x0;
    z(i) = x(i) + randi([-100, 100], 1, 1);
end
save('z.mat', 'z');

plot(1:num, z, 'g', 'LineWidth', 2);
hold on;
plot(1:num, x, 'k', 'LineWidth', 2);

數據分佈圖如下，其中黑色線表示真實的狀態值，而綠色添加隨機數噪聲之後的生成的測量值，這時候的噪聲已經很大了：

example result11

那麼在執行上面的卡爾曼濾波算法，裏面參數不變：

example result12

看上面的結果還能擬合的這麼好，在這麼大的噪聲前提下，這說明啥問題，現在解釋一下，主要有下面兩方面原因：

第一是設置了一個正確的 x0x_0 ；

第二是設置了一個比較大的 RR ；

這會導致什麼情況呢？卡爾曼濾波算法基本上會忽略掉測量值的作用，因爲 RR 太大表示測量誤差太大，系統不相信該值，在示例2中也討論過，系統會儘可能相信過程中計算的飛機海拔，同時還提供了一個完美的 x0x_0 ，所以肯定完美的擬合了。

那麼好在進行卡爾曼濾波算法時設置 x0=200x_0 = 200，終於不是完美的擬合了吧，明顯紅色是另一條函數曲線，所以目前僅僅是利用了過程計算的信息，測量的值幾乎忽略掉了，當然本來也是當前的測量值誤差也太大：

example result13

在該基礎上調整 R=1R = 1 ， 可以看到下面效果要好很多了，主要是設置初始的 x0x_0 差距太大，所以該示例也給以後使用提供了經驗：

example result14

爲什麼說初始的 x0x_0 會有這麼大影響了，回到剛纔的數據（添加高斯白噪聲的），設置精確的 R=900R = 900 ，然後設置 x0=200x_0 = 200 ，得到下面的結果，雖然說 RR 設置是非常精確的，但是結果卻不理想：

example result15

然後和上面一樣調整 R=1R = 1 ， 可以看到下面效果要好很多了，主要是設置初始的 x0x_0 差距太大，所以該示例也給以後使用提供了經驗：

example result16

根據上面的測試以及示例2中的結果，如果設置的 x0x_0 的值和真實相差太大，其實一般情況下的話，該值是根據場景自己來定義的，不會出現大的問題。但是在特殊情況下，如果該值的設置偏移比較大，那麼就不能在進行卡爾曼濾波算法時設置太大的 RR 值，因爲這種情況下，會導致測量噪聲太大，系統不會相信提供的測量值，而僅僅是相信過程值，但是又因爲過程值的初始值都是錯誤的，有很大的偏移，所以需要設置一個小的 RR 值，這樣可以得到一個正確的估計。其實從常理上也可以推斷，既然有測量值，就要儘可能的相信它，這其實是唯一的輸入來源，所以 RR 不能設置特別大。在該示例中越小越好，可以把上面兩組數據的 R=0.0000001R = 0.0000001 （不能設置爲0） ，會發現效果特別好，幾乎是完全擬合了，結果如下：

example result17 example result18

但還有種情況就是誤差特別特別大，此時也不能設置的太小，不然的話會過於相信測量數據，在示例2中已經證明了。

但是看到上面的結果，誤差其實也不小了，而且提供的初值偏移很大，還能這麼好的被擬合，說明卡爾曼濾波算法太強大了。

然後對這兩組數據調整一下 P0=0.001P_0 = 0.001 （設置爲0效果一樣），其他參數 x0=200x_0 = 200 ，R=1R = 1 ，結果如下，上面也提到該值如果太小，也會導致 KkK_k 的值太小，所以在估計過程中不會去相信測量值：

example result19 example result20

上面也提到修改 Q=1Q = 1 （設置太小都不會起作用） 可能會改變結果，但是手動的引入過程噪聲，說明不能相信過程處理，那麼在這種情況下，系統會極大的相信測量值，波動太大，如下：

example result21 example result22

所以根據上面對各個參數的設置調整實驗，在實際使用中要儘可能設置一個合理的 P0P_0 ，然後爲了更好的利用測量值，RR 的值不能設置太大也不能太小，儘可能合理。當然如果系統的初始量可以儘可能的設置正確是最好了，這樣才能加快收斂過程。對於 QQ 一般來說是設置爲0或者是很小的數，因爲表明過程變化就是符合這個線性方程的，只是有很小的噪聲擾動，設置太大，過程方程就利用不上了。而關於其他的參數 A,B,HA, B, H 就根據具體實際情況進行設置。

簡單示例4

對示例3進行擴展，上面僅僅是估計飛機海拔，在這裏估計飛機海拔(altitude)，飛機航向(heading)，飛機速度(airspeed)三個變量，然後測量儀器分別是用於測量海拔的氣壓計(barometer)和GPS（注意有兩個測量儀器，需要融合），用於測量航向的羅盤(compass)，用於測量速度的皮託流速測定管(pitot tube)。

這種情況下會變得很複雜，基本模型中的測量方程如下：
[barometerkcompasskpitotkgpsk]=[100010001100][altitudek−1headingk−1airspeedk−1] \left[ \begin{array} { c } { \text {barometer} _ { k } } \\ { \text {compass} _ { k } } \\ { \text {pitot} _ { k } } \\ { g p s _ { k } } \end{array} \right] = \left[ \begin{array} { c c c } { 1 } & { 0 } & { 0 } \\ { 0 } & { 1 } & { 0 } \\ { 0 } & { 0 } & { 1 } \\ { 1 } & { 0 } & { 0 } \end{array} \right] \left[ \begin{array} { c } { \text {altitude} _ { k - 1 } } \\ { \text {heading} _ { k - 1 } } \\ { \text {airspeed} _ { k - 1 } } \end{array} \right]
本示例主要聚焦在傳感器數據融合計算上，所以簡化上面的模型，還是隻顧及飛機海拔，但是利用氣壓計以及gps兩個傳感器的測量值來進行估計。

參數設置

對於所有的變量，本場景下都是都是以向量和矩陣的形式：

uu 是被忽略的；

A=0.98A = 0.98 在該場景下，假設每個時刻飛機海拔下降2%，所以當前時刻飛機海拔是上一時刻飛機海拔的0.98（不考慮噪聲的話）；

H=[11]H = \begin{bmatrix} 1 \\ 1 \end{bmatrix} 多個傳感器融合；

x^0=1000\hat{x}_0 = 1000 設置最初的狀態爲1000，最初的飛機海拔；

P0=1P_0 = 1 上面提到過不能設置爲0，可以試一下，最後所有估計的狀態都是0了，只要設置不爲0結果就可以正確收斂；

Q=0Q = 0 在該示例中生成的數據是沒有過程噪聲的；

R=[90000900]R = \begin{bmatrix} 900 & 0 \\ 0 & 900 \end{bmatrix} 測量海拔的傳感器提供不同程度的精度，標準差是30，那麼方差爲900，氣壓計和GPS的噪聲分佈相同，但是獨立分佈的。

基本模型
xk=Axk−1+Buk−1+wk=0.98∗xk−1 \begin{aligned} x_k & = Ax_{k-1} + Bu_{k-1} + w_k \\ & = 0.98 * x_{k-1} \end{aligned}

zk=Hxk+vk=[11]∗xk+vk \begin{aligned} z_k & = Hx_k + v_k \\ & = \begin{bmatrix} 1 \\ 1 \end{bmatrix} * x_k + v_k \end{aligned}

基本公式

Time Update Measurement Update
x^kˉ=0.98∗x^k−1\bar{\hat{x}_k} = 0.98 * \hat{x}_{k-1} Kk=Pkˉ[11]([11]Pkˉ[11]+[90000900])−1K_k = \bar{P_k}\begin{bmatrix} 1 & 1 \end{bmatrix}(\begin{bmatrix} 1 \\ 1 \end{bmatrix}\bar{P_k}\begin{bmatrix} 1 & 1 \end{bmatrix} + \begin{bmatrix} 900 & 0 \\ 0 & 900 \end{bmatrix})^{-1}
Pkˉ=0.98∗Pk−1∗0.98\bar{P_k} = 0.98 * P_{k-1} * 0.98 x^k=x^kˉ+Kk(zk−[11]x^kˉ)\hat{x}_k = \bar{\hat{x}_k} + K_k(z_k - \begin{bmatrix} 1 \\ 1 \end{bmatrix}\bar{\hat{x}_k})
Pk=(1−Kk[11])PkˉP_k = (1 - K_k\begin{bmatrix} 1 \\ 1 \end{bmatrix})\bar{P_k}

注意其中 xx 和 PP 都是標量，z∈R2×1z \in R^{2 \times 1} 和 K∈R1×2K \in R^{1 \times 2} 。

仿真實驗

在這裏，設置最初的飛機海拔爲1000 ，其他時刻的真實海拔爲上一時刻海拔的0.98。然後去模擬生成100個（再多的話沒意義，都是0了）單獨的測量數據 zkz_k ，對每一個測量值隨機生成測量噪聲添加到該時刻的真實海拔上生成測量值，當然是對氣壓計和GPS單獨模擬生成，同時可能需要做多組實驗，預先生成測量集合可以保持每組實驗數據是完全相同的。

生成數據代碼如下，把生成的數據保存下來，生成具有隨機性，這裏結果的數據見附件altitude_z.mat ，後面運行卡爾曼濾波算法時不再重新生成，直接加載：

x0 = 1000;
num = 100;
x = [];
for i=1:num
    x0 =  x0 * 0.98;
    x(i) = x0;
end
bnoise = normrnd(0, 30, num, 1);
gnoise = normrnd(0, 30, num, 1);
z = [bnoise + x',gnoise + x'];
save('z.mat', 'z');

plot(1:num, z(:, 1), 'g', 'LineWidth', 2);
hold on;
plot(1:num, z(:, 2), 'm', 'LineWidth', 2);
legend('barometer', 'gps')
plot(1:num, x, 'k', 'LineWidth', 2);

數據分佈圖如下，其中黑色線表示真實的狀態值，而綠色添加白色高斯噪聲之後的生成的氣壓計測量值，洋紅色是添加白色高斯噪聲之後的生成的GPS測量值：

example result23

基本的卡爾曼濾波算法代碼：

x0 = 1000;
num = 100;
x = [];
for i=1:num
    x0 =  x0 * 0.98;
    x(i) = x0;
end
mat = load('z.mat');
z = mat.z;

xp = [1000]; Pp = [1]; xpp = [1000]; Ppp = [1]; K = [0 0];
A = 0.98; H = [1 1]'; Q = 0; R = [900 0;0 900];
for k=2:num+1
    xp(k) = A*xpp(k-1);
    Pp(k) = A*Ppp(k-1)*A' + Q;
    K = [K; Pp(k)*H'/(H*Pp(k)*H' + R)];
    xpp(k) = xp(k) + K(k, :)*(z(k-1, :)' - H*xp(k));
    Ppp(k) = (1 - K(k, :)*H)*Pp(k);
end

plot(1:num, z(:, 1), 'g', 'LineWidth', 2);
hold on;
plot(1:num, z(:, 2), 'm', 'LineWidth', 2);
legend('barometer', 'gps')
plot(1:num, x, 'k', 'LineWidth', 2);
plot(1:num, xpp(2:num+1), 'r', 'LineWidth', 2);
xlabel('Iteration');  
ylabel('Altitude');

結果如下，可以看出來雖然測量值的噪聲比較大，但是利用卡爾曼濾波算法還是很快收斂到正確的值，黑色線是真實的飛機海拔，紅色線是卡爾曼濾波算法估計的海拔，可以看到黑色線幾乎被全部遮擋住了，說明完美擬合了：

example result24

和示例3一樣設置 x0=200x_0 = 200 ，然後隨後設置 R=1,0.0000001R=1, 0.0000001 得到下面三個結果，和示例3結論一致：

example result25 example result26 example result27

簡單示例5

再給出一個簡單的小栗子，大家很熟悉的自由落體運動，在這裏測量值只是針對高度。

參數設置

對於所有的變量，本場景下都是都是以向量和矩陣的形式：

uu 該部分是加速度部分，下面給出模型的時候會給出具體值；

xx 在該場景下表示需要估計物體的高度以及當前速度；

AA 和 BB 在下面後給出具體的值；

H=[10]H = \begin{bmatrix} 1 & 0 \end{bmatrix} 在該場景下需要估計物體高度以及速度，但測量值只有高度，所以忽略速度項；

x^0=[1000]\hat{x}_0 = \begin{bmatrix} 100 \\ 0 \end{bmatrix} 設置最初的物體的高度爲100，速度爲0；

P0=[1111]P_0 = \begin{bmatrix} 1 & 1 \\ 1 & 1 \end{bmatrix} 上面提到過不能設置爲0，可以試一下，最後所有估計的狀態都是0了，只要設置不爲0結果就可以正確收斂；

Q=0Q = 0 在該示例中生成的數據是沒有過程噪聲的；

R=1R = 1 測量高度誤差項協方差。

基本模型

過程模型，包括高度（這裏表示的是下降的高度，所以當前高度等於上一高度加上該時間段內上升的高度，在這裏是下降，所以速度和加速度都是負值）以及速度：
yk=yk−1+vk−1Δt+12a(Δt)2vk=vk−1+aΔt y_k = y_{k-1} + v_{k-1}\Delta{t} + \frac{1}{2}a(\Delta{t})^2 \\ v_k = v_{k-1} + a\Delta{t}
所以把這兩個狀態值統一起來，（這裏的 vv 表示速度不是測量誤差）：
xk=[ykvk]=[1Δt01]xk−1+[Δt22Δt]a \begin{aligned} x_k & = \begin{bmatrix} y_k \\ v_k\end{bmatrix} \\ & = \begin{bmatrix} 1 & \Delta{t} \\ 0 & 1 \end{bmatrix}x_{k-1} + \begin{bmatrix} \frac{\Delta{t}^2}{2} \\ \Delta{t} \end{bmatrix}a \end{aligned}
而且在這裏 Δt=1\Delta{t} = 1 和 a=−9.81a = -9.81 ，所以上面的 A=[1101]A = \begin{bmatrix} 1 & 1 \\ 0 & 1 \end{bmatrix} 和 B=[121]B = \begin{bmatrix} \frac{1}{2} \\ 1 \end{bmatrix}

所以得到最終的模型：
xk=[1101]xk−1+[121]∗−9.81 x_k = \begin{bmatrix} 1 & 1 \\ 0 & 1 \end{bmatrix}x_{k-1} + \begin{bmatrix} \frac{1}{2} \\ 1 \end{bmatrix}* -9.81

zk=Hxk+vk=[10]∗xk+vk \begin{aligned} z_k & = Hx_k + v_k \\ & = \begin{bmatrix} 1 & 0 \end{bmatrix} * x_k + v_k \end{aligned}

基本公式

Time Update Measurement Update
x^kˉ=[1101]x^k−1+[121]∗−9.81\bar{\hat{x}_k} = \begin{bmatrix} 1 & 1 \\ 0 & 1 \end{bmatrix}\hat{x}_{k-1} + \begin{bmatrix} \frac{1}{2} \\ 1 \end{bmatrix}* -9.81 Kk=Pkˉ[10]([10]Pkˉ[10]+1)−1K_k = \bar{P_k}\begin{bmatrix} 1 \\ 0 \end{bmatrix}(\begin{bmatrix} 1 & 0 \end{bmatrix}\bar{P_k}\begin{bmatrix} 1 \\ 0 \end{bmatrix} + 1)^{-1}
Pkˉ=[1101]Pk−1[1011]\bar{P_k} = \begin{bmatrix} 1 & 1 \\ 0 & 1 \end{bmatrix}P_{k-1}\begin{bmatrix} 1 & 0 \\ 1 & 1 \end{bmatrix} x^k=x^kˉ+Kk(zk−[10]x^kˉ)\hat{x}_k = \bar{\hat{x}_k} + K_k(z_k - \begin{bmatrix} 1 & 0 \end{bmatrix}\bar{\hat{x}_k})
Pk=(I−Kk[10])PkˉP_k = (I - K_k\begin{bmatrix} 1 & 0 \end{bmatrix})\bar{P_k}

注意其中 x∈R2×1x \in R^{2 \times 1} ，P∈R2×2P \in R^{2 \times 2} ，K∈R2×1K \in R^{2 \times 1} 。

仿真實驗

在這裏，給出6組測量數據，如下：

TIME(ms) 1 2 3 4 5 6
HEIGHT(V) 127.0 115.3 110.9 72.4 50.7 0.3

基本的卡爾曼濾波算法的代碼：

num = 6;
z = [ 127.0, 115.3, 110.9, 72.4, 50.7, 0.3];
x = [125];
xp = [100 0]'; Pp{1} = [1 1;1 1]; xpp = [100 0]'; Ppp{1} = [1 1;1 1]; K = [0 0]';
A = [1 1;0 1]; B = [1/2, 1]'; u = -9.81; H = [1 0]; Q = 0; R = 10;
for k=2:num
    x = [x x(1)-1/2*9.81*(k-1)^2];
    xp = [xp A*xpp(:, k-1) + B*u];
    Pp{k} = A*Ppp{k-1}*A' + Q;
    K = [K Pp{k}*H'/(H*Pp{k}*H' + R)];
    xpp = [xpp xp(:, k) + K(:, k)*(z(k) - H*xp(:, k))];
    Ppp{k} = (1 - K(:, k)*H)*Pp{k};
end

plot(0:num-1, z, 'g', 'LineWidth', 2);
hold on;
plot(0:num-1, x(1:num), 'k', 'LineWidth', 2);
plot(0:num-1, xpp(1,1:num), 'r', 'LineWidth', 2);
xlabel('height');  
ylabel('time');

這個例子和之前的比較類似，就不詳細介紹了。

發表評論
所有評論
還沒有人評論，想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章

Matlab機器人工具箱（二）

時間與運動 上一章主要介紹了在二維和三維空間中如何描述位姿。這一章則將上章拓展到對象，其位姿是時間的函數。 分兩個部分，第一個部分主要討論如何產生一系列短暫的位姿序列，一條足夠光滑的從起始位姿到終止位姿的軌跡； 第二部分主要討論位

jameshater 2020-07-07 23:59:53

機器人常用軟件

常用控制軟件： ActivMedia Robotics Interface for Applications - ARIA is an OO C++ library for controlling ActivMedia mobile

阿困 2020-07-07 20:25:03

免費語言學習服務Duolingo正式進入亞洲市場

    Duolingo是一個所譯即所學的語言學習工具，讓人們免費學習外語的同時也利用衆包完成其他網頁內容的翻譯。在今年二月，Duolingo 曾宣佈獲得由 Kleiner Perkins 領投的2000 萬美元B 輪融資。並隨

狗熊不偷白菜 2020-07-07 09:30:06

轉：機器人工程師學習計劃（YY碩）（後悔自己沒有早點看到強力推薦）

申明：轉載自知乎——YY碩 地址：https://zhuanlan.zhihu.com/p/22266788 作者：YY碩 鏈接：https://zhuanlan.zhihu.com/p/22266788 來源：知乎 著作

Gong_Shuai 2020-07-07 05:30:28

diego1# 突破局域網限制，實現互聯網範圍內控制你的ROS機器人

更多創客作品，請關注筆者網站園丁鳥，蒐集全球極具創意，且有價值的創客作品 ROS機器人知識請關注，diegorobot 業餘時間完成的一款在線統計過程分析工具SPC，及SPC知識分享網站qdo 1.概述 ROS機器人系統有一個侷

DiegoRobot 2020-07-07 05:24:40

ros costmap框架分析

一、costmap類型 global_costmap：全局代價地圖，用於全局路徑規劃。 locat_costmap：局部代價地圖，用於避障。 二、什麼是costmap  簡單來講，costmap就是一張代價地圖，地圖數據是一個unsign

码农不是农 2020-07-06 21:59:08

Python3之微信小機器人召喚術

最近業餘，入了python的坑。 結果越陷越深··· python真的很好喫啊！嘿嘿^o^ 本篇將會爲大家詳細介紹一個不超過60行代碼的微信小機器人，另外特別說明下，因爲考慮到官方的支持因素，所以我在學習python之前，直接選擇的是3.

陆玖神通 2020-07-06 20:47:45

新松機器人：【示教編程】+【PC離線編程】+【PLC遠程IO】相結合

項目背景 先說一下項目背景，項目要求潮溼的工業環境，在保證機器人末端跟流水線保持相對靜止的情況下，控制末端執行器沿XZXZXZ平面內的點進行移動。 功能要求： 機器人末端執行器實現對工廠流水線的隨動，其中流水線沿下圖YYY軸方向

$南山种豆$ 2020-07-06 12:31:00

Golang三行代碼發送釘釘機器人消息

dingtalk 釘釘機器人消息封裝，三行代碼發送釘釘消息——Golang實現，簡化日常通過釘釘羣機器人發送提示、報警消息操作。 目前自定義機器人支持 文本（text） 鏈接（link） markdown ActionCard

Liyuxinger 2020-07-06 03:33:12

四、達爾文機器人開發指南(針對中國機器人比賽、華北五省機器人比賽)-----達爾文官方框架解讀

這是我開發過程中，參考的第二個博客，同樣爲了好理解，這裏我在歸納一下。以下是達爾文的各模塊圖： 一、DARwin 機器人Frame代碼簡析（一） 1. CM730.cpp 主要定義了一些和控制板CM730（OP2實際使用的控制板是740

AA_BUYI工作室 2020-07-05 23:05:23

二、達爾文機器人開發指南(針對中國機器人比賽、華北五省機器人比賽)-----達爾文基礎使用說明

目錄 一、達爾文實例程序 二、LINUX基礎命令彙總（開發過程用） 1.top命令 2.sudo kill 1722(這裏是要關掉的線程號) 3.sudo su 4.rm -r demo(main) 5.make 6./demo 7.li

AA_BUYI工作室 2020-07-05 23:05:23

機器人平臺ABC Robot開發初探

      百度於2018年上線了專門針對服務類機器人的打包SDK，鏈接：http://robot.baidu.com/#/robotPlatform，這套SDK集成了百度的語音智能，視覺智能，對話智能，數據智能等AI平臺，讓開發者可以快

xjp13 2020-07-05 03:16:18

Tof，結構光，三角測距，RGBD，雙目，激光雷達，毫米波雷達一文總結（一）

Tof，結構光，三角測距，RGBD，雙目，激光雷達，毫米波雷達一文總結距離測量算法解析TOF 飛行時間測距法超聲波毫米波雷達激光雷達 最近在做一些無人車相關的工作，對其中的一些基礎技術做了些總結和歸納，主要涉及以下技術，將會分兩

Lynn_Chan 2020-07-05 02:58:26

多項式參數曲線擬合C++實現

#include <ros/ros.h> #include <tf/tf.h> #include <iostream> #include <map> #include <math.h> #include <vector> #includ

zgrobot 2020-07-04 12:07:56

轉載《ubuntu 下pycharm 構建ROS編程環境》

4    我遇到的問題。 我在添加msg後，import始終沒法找到我的msg模塊，原因是因爲python_path中不包含你的msg的py文件。查看後發現，在～/your workspace/devel/lib/python2.7/di

pxzsky 2020-07-03 21:23:22