一文詳解卡爾曼濾波

作者：嗣音 | 來(lái)源：古月居

記錄一下對卡爾曼濾波的理解。

卡爾曼濾波（Kalman Filter），以下簡(jiǎn)稱(chēng)KF，是由Swerling（1958）和Kalman（1960）作為線(xiàn)性高斯系統（linear Gaussian system）中的預測和濾波技術(shù)而發(fā)明的，是用矩陣來(lái)定義的。

KF實(shí)現了連續狀態(tài)的置信度計算。它不適用于離散或混合狀態(tài)空間。

參考《概率機器人》、《卡爾曼濾波原理及應用-MATLAB仿真》

卡爾曼濾波屬于是基于貝葉斯最大后驗估計（MAP）的推論。此篇之前所銜接的博客是《概率基礎：貝葉斯濾波》。

一、線(xiàn)性高斯系統

1.1 線(xiàn)性系統

線(xiàn)性系統和非線(xiàn)性系統的主要區別在于其輸入和輸出之間的關(guān)系以及其遵循的原理。線(xiàn)性系統相對簡(jiǎn)單，容易分析和控制，而非線(xiàn)性系統則更復雜，具有更多不確定性和不穩定性。

所謂線(xiàn)性系統即指滿(mǎn)足疊加性與齊次性的系統,反之即為非線(xiàn)性系統。

線(xiàn)性系統疊加原理：對于輸入信號的線(xiàn)性組合，系統將產(chǎn)生相應的輸出信號的線(xiàn)性組合。

線(xiàn)性系統齊次原理：當輸入信號放大 k 倍時(shí)，輸出信號也會(huì )放大 k 倍。

平時(shí)可以根據這幾點(diǎn)判斷線(xiàn)性or非線(xiàn)性

1.線(xiàn)性方程是指方程中只包含一次項，而非線(xiàn)性方程則不是這樣的形式，可能包含二次、三次或其它次數的項。

2.如果方程中包含有指數或冪，并且指數或冪不等于 1，那么它就是一個(gè)非線(xiàn)性方程。

3.如果方程中存在變量與變量的乘積或除法，那么它就是一個(gè)非線(xiàn)性方程。

舉個(gè)例子，2x+3y=5 是一個(gè)線(xiàn)性方程，它只包含了一次項；而 x^2+y^2=4 是一個(gè)非線(xiàn)性方程，因為它包含了平方項；xy=2 是一個(gè)非線(xiàn)性方程，因為它包含了兩個(gè)變量的乘積。

1.2 高斯分布

高斯分布(Gaussian Distribution)又稱(chēng)正態(tài)分布(Normal Distribution)。

若隨機變量X服從一個(gè)數學(xué)期望（均值）為 μ，方差為 σ 的正態(tài)分布，則記為 X~N(μ, σ)。

高斯分布的概率密度函數為正態(tài)分布，期望值（均值） μ 決定了其位置，其標準差 σ 決定了分布的幅度。

μ = 0、σ = 1；μ = 2、σ = 0.8；μ = -2、σ = 1.5;的分布曲線(xiàn)如下：

matlab代碼：

%% 高斯分布(Gaussian Distribution)
% 生成數據點(diǎn)范圍
x = -1010;


% 定義不同的高斯分布的均值和標準差
mu1 = 0;
sigma1 = 1;


mu2 = 2;
sigma2 = 0.8;


mu3 = -2;
sigma3 = 1.5;


% 計算不同高斯分布在數據點(diǎn)范圍內的概率密度函數值
pdf1 = normpdf(x, mu1, sigma1);
pdf2 = normpdf(x, mu2, sigma2);
pdf3 = normpdf(x, mu3, sigma3);


% 繪制高斯分布曲線(xiàn)
figure;
plot(x, pdf1, 'b-', 'LineWidth', 1.5, 'DisplayName', 'μ=0 σ=1');
hold on;
plot(x, pdf2, 'r--', 'LineWidth', 1.5, 'DisplayName', 'μ=2 σ=0.8');
plot(x, pdf3, 'g-.', 'LineWidth', 1.5, 'DisplayName', 'μ=-2 σ=1.5');


% 添加圖例和標簽
legend('Location', 'best');
xlabel('x');
ylabel('Probability Density');
title('Comparison of Gaussian Distributions');
grid on;
hold off;

1.3 高維高斯分布

高維高斯分布(Multivariate Gaussian Distribution)又稱(chēng)高維正態(tài)分布(Multivariate Normal Distribution)。

與高斯分布不同的是，其數學(xué)期望（均值）為 μ 為一個(gè) 1xn 的矩陣，方差這里叫做協(xié)方差 cov 為一個(gè) nxn 的矩陣。

μ = [2, 3]、cov = [1.5, 0.7; 0.7, 2.5]；μ = [-1, 4]、σ = [3, -1.2; -1.2, 2]；的分布曲線(xiàn)如下：

matlab代碼：

%% 高維高斯分布(Multivariate Gaussian Distribution)
% 設置均值向量和協(xié)方差矩陣
mu1 = [2, 3];     % 第一個(gè)高斯分布的均值向量
cov1 = [1.5, 0.7;  % 第一個(gè)高斯分布的協(xié)方差矩陣
    0.7, 2.5];


mu2 = [-1, 4];    % 第二個(gè)高斯分布的均值向量
cov2 = [3, -1.2;   % 第二個(gè)高斯分布的協(xié)方差矩陣
    -1.2, 2];


% 生成數據點(diǎn)范圍
x = -55;
y = -58;


% 創(chuàng  )建網(wǎng)格數據點(diǎn)
[X, Y] = meshgrid(x, y);


% 將網(wǎng)格數據點(diǎn)組合成行向量
points = [X(:), Y(:)];


% 計算不同高斯分布在數據點(diǎn)范圍內的概率密度函數值
pdf1 = mvnpdf(points, mu1, cov1);
pdf2 = mvnpdf(points, mu2, cov2);


% 將概率密度函數值變換回網(wǎng)格形式
PDF1 = reshape(pdf1, size(X));
PDF2 = reshape(pdf2, size(X));


% 繪制高斯分布曲線(xiàn)
figure;
contour(X, Y, PDF1, 'LineColor', 'blue', 'LineWidth', 1.5, 'DisplayName', 'Gaussian 1');
hold on;
contour(X, Y, PDF2, 'LineColor', 'red', 'LineWidth', 1.5, 'DisplayName', 'Gaussian 2');


% 添加圖例和標簽
legend('Location', 'best');
xlabel('x');
ylabel('y');
title('Comparison of Multivariate Gaussian Distributions');
grid on;
hold off;

1.4 方差與協(xié)方差

方差與協(xié)方差的理解：

https://zhuanlan.zhihu.com/p/518236536

1.5 線(xiàn)性高斯系統

KF用矩陣參數表示置信度：在時(shí)刻t，置信度用均值μ(t)和方差Σ(t)表達。除了貝葉斯濾波的馬爾可夫假設（回想隱馬爾可夫模型 或 動(dòng)態(tài)貝葉斯網(wǎng)絡(luò )），還需要具有三個(gè)特征，則后驗就是高斯的。

1.狀態(tài)轉移概率p(x(t) | u(t), x(t-1))必須是帶有隨機高斯噪聲的參數的線(xiàn)性函數：

x(t) = A(t).x(t-1) + B(t).u(t) + ε(t)
式中，x(t)和x(t-1)為狀態(tài)向量，其維數為 n ；u(t)為時(shí)刻t的控制向量，其維數為 m；A(t)為 n x n 的方陣；B(t)為 n x m的矩陣。

ε(t) 是一個(gè)高斯隨機向量，表示由狀態(tài)轉移引入的不確定性，其維數與狀態(tài)向量相同為 n，均值為0，方差用 R(t) 表示。

2.測量概率p(z(t) | x(t))也與帶有高斯噪聲的自變量呈線(xiàn)性關(guān)系：

z(t) = C(t).x(t) + δ(t)
式中，x(t)為狀態(tài)向量，其維數為 n ；z(t) 為測量向量，其維數為 k ；C(t)為 k x n 的矩陣，均值為0，方差用 Q(t) 表示。

3.初始置信度bel(x0)必須是正態(tài)分布的。

二、卡爾曼濾波算法

2.1 算法理解

其推導這里不做記錄，可以參照《概率機器人》

使用過(guò)程就是這五個(gè)黃金公式一直套就對了

1.輸入：控制向量u(t)和測量向量z(t)；輸出：時(shí)刻t的置信度，均值為μ(t)和方差為Σ(t)。

2.在第2第3行，計算預測置信度_bel(x(t))的均值_μ(t)和方差為_(kāi)Σ(t)。

均值利用狀態(tài)轉移函數進(jìn)行計算，把x(t-1)替換為u(t-1)。

方差是通過(guò)線(xiàn)性矩陣A(t)考慮了當前狀態(tài)依賴(lài)前一狀態(tài)的的事實(shí)進(jìn)行計算的。因為方差是一個(gè)二次型的，所以該矩陣兩次相乘得到方差。

3.在第4到第6行，通過(guò)綜合測量z(t)順序地轉換成期望的置信度bel(x(t)).

第4行計算的變量矩陣 K(t) 叫作卡爾曼增益（Kalman gain），它明確了測量綜合到新的狀態(tài)估計的程度。

第5行通過(guò)根據卡爾曼增益矩陣K(t)、真實(shí)測量向量z(t)的偏差及根據測量方程進(jìn)行調節來(lái)處理均值。

第6行計算后驗置信度bel(x(t))的方差，調整由測量引起的信息增益。

2.2 特點(diǎn)描述

1.由于Kalman濾波算法將被估計的信號看作在白噪聲作用下一個(gè)隨機線(xiàn)性系統的輸出，并且其輸入、輸出關(guān)系是由狀態(tài)方程和輸出方程在時(shí)間域內給出的。

因此這種濾波方法不僅適用于平穩隨機過(guò)程的濾波，而且特別適用于非平穩或平穩馬爾可夫序列的濾波，所以其應用是十分廣泛的。

2.Kalman濾波算法是一種時(shí)間域濾波的方法，采用狀態(tài)空間描述系統。

系統的過(guò)程噪聲和測量噪聲并不是需要濾除的對象，它們的統計特性正是估計過(guò)程中需要利用的信息，而被估計量和觀(guān)測量在不同時(shí)刻的一、二階矩卻是不必要知道的。

3.由于 Kalman濾波算法的基本方程是時(shí)間域內的遞推形式，其計算過(guò)程是一個(gè)不斷“預測-修正”的過(guò)程。

在求解時(shí)不要求存儲大量數據，并且一旦觀(guān)測到了新的數據，隨即可以算得新的濾波值，因此這種濾波方法非常適合于實(shí)時(shí)處理、計算機實(shí)現。

4.由于濾波器的增益矩陣與觀(guān)測無(wú)關(guān)，因此它可以預先離線(xiàn)算出，從而可以減少實(shí)時(shí)在線(xiàn)計算量。

在求濾波器增益時(shí)，要求一個(gè)矩陣的逆，它的階數只取決于觀(guān)測方程的維數，而該維數通常很小，這樣，求逆運算是比較方便的。

另外，在求解濾波器增益的過(guò)程中，隨時(shí)可以算得濾波器的精度指標 P ，其對角線(xiàn)上的元素就是濾波誤差向量各分量的方差。

2.3 噪聲矩陣的處理

狀態(tài)轉移方程：x(t) = A(t).x(t-1) + B(t).u(t) + ε(t)；觀(guān)測方程：z(t) = C(t).x(t) + δ(t)

其中我們一般假設，過(guò)程噪聲 ε(t) 是均值為0方差為 R 的白噪聲，觀(guān)測噪聲 δ(t) 是均值為0方差為 Q 的白噪聲。

如何確定 R 和 Q 呢？

1.對于過(guò)程噪聲方差 R

狀態(tài)轉移方程所代表的意義就是：本階段的狀態(tài)是上一階段狀態(tài)和上一階段決策的結果。

例如，在目標跟蹤系統中，過(guò)程噪聲往往是路面摩擦力、空氣阻力等因素造成的；在溫度測量系統中，過(guò)程噪聲往往是由于人體干擾、陽(yáng)光照射、風(fēng)等因素造成的。

要準確獲取過(guò)程噪聲方差 R 比較困難，可以通過(guò)對比試驗獲得。

例如，機器人小車(chē)在光滑的玻璃上與粗糙的路面上行駛，兩者對比就可以獲得在路面上的阻力因素，從而獲得過(guò)程噪聲方差 R。

2.對于觀(guān)測噪聲方差 Q

觀(guān)測噪聲與傳感器精度相關(guān)。

例如，一個(gè)溫度計的測量誤差是+-1℃；學(xué)生用尺子量距離誤差是+-1mm；體重計測量體重的誤差是+-1g。

根據這類(lèi)信息，我們可以大致知道他們測量噪聲的大小。

一般地，觀(guān)測噪聲的方差 Q 是一個(gè)統計意義上的參數：對傳感器測量的數據經(jīng)過(guò)長(cháng)期的概率統計，得到它測量噪聲的方差。

三、實(shí)例一：溫度測量

3.1 問(wèn)題描述

1.假設我們要研究的對象是一個(gè)房間的溫度，其溫度大概在25℃左右，會(huì )小幅度波動(dòng)，每隔一分鐘采樣一次，以 t 表達時(shí)序。

問(wèn)題是要通過(guò)卡爾曼濾波估算房間真實(shí)溫度。

這個(gè)實(shí)例的代表意義是，他是一個(gè)一維的線(xiàn)性高斯系統問(wèn)題。

2.受光照、空氣流動(dòng)影響，真實(shí)溫度會(huì )隨環(huán)境變化，存在過(guò)程噪聲。

過(guò)程噪聲是均值為0、方差 R=0.01的高斯分布；其狀態(tài)轉移方程：x(t) = A(t).x(t-1) + B(t).u(t) + ε(t)

A=1，B=0，ε均值為0、方差R=0.01

3.用溫度計測量，誤差為+-0.5℃，并從產(chǎn)品說(shuō)明上得知其方差為0.25，也就是說(shuō)有測量噪聲。

測量噪聲是均值為0、方差 Q=0.25的高斯分布；

其觀(guān)測方程：z(t) = C(t).x(t) + δ(t)

C=1，δ均值為0、方差Q=0.25

3.2 手動(dòng)解算

假定第 t-1 時(shí)刻溫度測量值為23.9℃，我們就假設推理到此時(shí)刻得到的溫度值均值 μ(t-1)=23.9，方差 Σ(t-1)=0.1^2=0.01

假定第 t 時(shí)刻溫度測量值為24.5℃

首先利用 t-1 時(shí)刻的溫度值預測第 t 時(shí)刻的溫度:

預測均值 _μ(t)=μ(t-1)=23.9

預測方差 _Σ(t)=Σ(t-1)+R=0.01+0.01=0.02

計算卡爾曼增益

K=_Σ(t)/(_Σ(t)+Q)=0.02/(0.02+0.25)=0.0741

則此時(shí)利用 t 時(shí)刻的觀(guān)測值，得到溫度計的估值為:

均值 μ(t)=_μ(t)+K.(z(t)-_μ(t))=23.9+0.0741x(24.5-23.9)=23.915；

方差 Σ(t)=(1-K)*_Σ(t)=(1-0.0741)x0.02=0.018518

后續重復此循環(huán)，不斷把方差遞歸

3.3 matlab實(shí)現

%% 根據系統描述，初始化一些值
clc;clear;
N = 300;          % 采樣次數


X = zeros(1,N);       % 狀態(tài)值 真值
Z = zeros(1,N);       % 觀(guān)測值 z(t)


X_kf = zeros(1,N);     % kf算法里的狀態(tài)均值 μ(t)
P_kf = zeros(1,N);     % kf算法里的狀態(tài)方差 Σ(t)


A = 1;           % 狀態(tài)轉移方程相關(guān)
C = 1;           % 測量方程相關(guān)
I = eye(1);         % 單位矩陣


R = 0.01;          % 過(guò)程噪聲方差
Q = 0.25;          % 測量噪聲方差


% 初值給定
X(1) = 25.1;
Z(1) = 24.9;
X_kf(1) = Z(1);       % 通過(guò)第一個(gè)觀(guān)測值來(lái)更新初始狀態(tài)均值 μ(1)
P_kf(1) = 0.01;       % 這里直接將過(guò)程噪聲方差作為初始方差（估摸著(zhù)）




%% 根據噪聲，模擬實(shí)際數據 和 測量數據
W = sqrt(R)*randn(1,N);   % 過(guò)程噪聲 ε(t)
V = sqrt(Q)*randn(1,N);   % 測量噪聲 δ(t)
for t = 2:N
  X(t) = A*X(t-1) + W(t-1);
  Z(t) = C*X(t) + V(t);
end




%% 卡爾曼濾波
for t = 2:N
  X_pre = A*X_kf(t-1);          % 預測狀態(tài)均值 _μ(t)
  P_pre = A*P_kf(t-1) + R;        % 預測狀態(tài)方差 _Σ(t)
  K = P_pre/(P_pre + Q);         % 更新卡爾曼增益 K
  X_kf(t) = X_pre + K*(Z(t) - C*X_pre);  % 更新?tīng)顟B(tài)均值 μ(t)
  P_kf(t) = (I - K*C)*P_pre;        % 更新?tīng)顟B(tài)方差 Σ(t)
end




%% 分析誤差項
Err_Messure=zeros(1,N);
Err_Kalman=zeros(1,N);
for t=1:N
  Err_Messure(t)=abs(Z(t)-X(t));
  Err_Kalman(t)=abs(X_kf(t)-X(t));
end




%% 畫(huà)圖
t=1:N;
figure('Name','Kalman Filter Simulation','NumberTitle','off');
plot(t,X,'-r',t,Z,'-k',t,X_kf,'-g');
legend('real','measure','kalman extimate');
xlabel('sample time (min)');
ylabel('temperature (℃)');
title('Kalman Filter Simulation');


figure('Name','Error Analysis','NumberTitle','off');
plot(t,Err_Messure,'-b',t,Err_Kalman,'-k');
legend('messure error','kalman error');
xlabel('sample time (min)');
ylabel('error (℃)');
title('Error Analysis');


figure('Name','Kalman Filter Variance','NumberTitle','off');
plot(t,P_kf,'-b');
xlabel('sample time (min)');
ylabel('Variance');
title('Kalman Filter Variance');

3.4 結果分析

真實(shí)值、測量值、KF得到的值如下，可以看到明顯的效果。

測量值、KF得到的值與真實(shí)值做差對比如下。

這個(gè)一維高斯分布里，其方差也是一維了，我們可以看到隨著(zhù)觀(guān)測數據的積累和濾波器的逐漸收斂方差趨于穩定。

四、實(shí)例二：自由落體運動(dòng)目標跟蹤

4.1 問(wèn)題描述

1.某一物體在重力作用下自由落體，有觀(guān)測裝置對其進(jìn)行檢測，采樣間隔1s。

需要估計該物體的運動(dòng)位移 x 和速度 v。

這個(gè)實(shí)例的代表意義是，它只有一個(gè)觀(guān)測，但解決的是一個(gè)2維的狀態(tài)估計問(wèn)題.

2.根據運動(dòng)學(xué)方程，該物體的狀態(tài)轉移方程為:

ε(t) = [0]T 也即過(guò)程噪聲為高斯分布，其均值為0、方差 R = [0]T ，物體的初始狀態(tài)為 [95, 1]T，初始方差為 [sqrt(100), 0; 0 ,sqrt(1)]

3. 給定觀(guān)測裝置，在測量時(shí)受到隨機干擾 δ(t) 影響，其方差為測量噪聲 Q = [1]T，觀(guān)測方程寫(xiě)為：

4.2 手動(dòng)解算

略，看matlab代碼及注釋。

4.3 matlab實(shí)現

printf("hello world!");

4.4 結果分析

真實(shí)值、測量值、KF得到的值如下，因為值比較大，所以這里不是很能看出什么。

測量值、KF得到的值與真實(shí)值做差對比如下，這個(gè)就能比較明顯的效果了。

五、實(shí)例三：船舶GPS導航定位系統

5.1 問(wèn)題描述

1.船舶出港沿著(zhù)某直線(xiàn)方向航行，采樣間隔?t。

需要估計船在x和y方向上的位置和速度為x(t)、vx(t)、y(t)、vy(t)

這個(gè)實(shí)例的代表意義是，它只有一個(gè)觀(guān)測，但解決的是一個(gè)4維的狀態(tài)估計問(wèn)題。

2.船舶動(dòng)力系統的控制信號u(t)是人為輸出的已知機動(dòng)信號；過(guò)程噪聲則來(lái)自于由海浪和海風(fēng)引起的隨機加速度。

這里不考慮船自身的動(dòng)力因素，也就是假設 u(t)=0。

過(guò)程噪聲ε(t)為高斯分布，其均值為0、方差為 R 。
根據運動(dòng)學(xué)方程，該物體的狀態(tài)轉移方程為:

3. 給定GPS觀(guān)測裝置觀(guān)測位置，民用GPS導航衛星播放的信號人為加入了高頻震蕩隨機干擾信號。

將干擾信號看做觀(guān)測噪聲δ(t)，假設其為零均值、方差為 Q 的白噪聲。

觀(guān)測方程寫(xiě)為：

5.2 手動(dòng)解算

略，看matlab代碼及注釋。

5.3 matlab實(shí)現

%% 根據系統描述，初始化一些值
clc;clear;
T = 1;          % 采樣步長(cháng) ?t
N = 80/T;         % 采樣次數
X = zeros(4,N);      % 狀態(tài)值X=[x y vx vy]T 真值 
Z = zeros(2,N);      % 觀(guān)測值Z=[Zx Zy]




R = (1e-2)*diag([0.5,1,0.5,1]); % 過(guò)程噪聲方差，diag對角矩陣
Q = 100*eye(2);         % 測量噪聲方差




A =[1,T,0,0;          % 狀態(tài)轉移方程相關(guān)
  0,1,0,0;
  0,0,1,T;
  0,0,0,1];


C = [1,0,0,0;          % 測量方程相關(guān)
  0,0,1,0];


I = eye(4);           % 單位矩陣


X_kf = zeros(4,N);     % kf算法里的狀態(tài)均值 μ(t)
P_kf = zeros(4,4,N);    % kf算法里的狀態(tài)方差 Σ(t)
Xkf = X_kf;
F=A;
H=C;
% 初值給定
X(:,1) = [-100,2,200,20];
Z(:,1) = [X(1,1),X(3,1)];
X_kf(:,1) = X(:,1);
P_kf(:,:,1) = eye(4);




%% 根據噪聲，模擬實(shí)際數據 和 測量數據
for t = 2:N
  X(:,t) = A*X(:,t-1) + sqrtm(R)*randn(4,1);
  Z(:,t) = C*X(:,t) + sqrtm(Q)*randn(2,1);
end




%% 卡爾曼濾波
P0 = eye(4);
for t=2:N
  X_pre = A*X_kf(:,t-1);
  P_pre = A*P_kf(:,:,t-1)*A' + R;
  K = P_pre*C'/(C*P_pre*C' + Q);
  X_kf(:,t) = X_pre + K*(Z(:,t) - C*X_pre);
  P_kf(:,:,t) = (eye(4) - K*C)*P_pre;
end




%% 分析誤差項
Err_Messure_x = zeros(1,N);
Err_Kalman_x = zeros(1,N);
Err_Kalman_v = zeros(1,N);


for t = 1:N
  Err_Messure_x(t) = RMS(X(:,t),Z(:,t)); % RMS：自定義函數，求歐拉距離的
  Err_Kalman_x(t) = RMS(X(:,t),X_kf(:,t));
end




%% 畫(huà)圖
t=1:N;
figure('Name','Kalman Filter Simulation','NumberTitle','off');
plot(X(1,:),X(3,:),'-k',Z(1,:),Z(2,:),'-b.',X_kf(1,:),X_kf(3,:),'-r+');
legend('real','measure','kalman extimate');     
xlabel('position x (m)');
ylabel('position y (m)');
title('Kalman Filter Simulation');


figure('Name','Error Analysis','NumberTitle','off');
plot(t,Err_Messure_x,'-g.',t,Err_Kalman_x,'-r.');
legend('messure error','kalman error');     
xlabel('sample time (s)');
ylabel('error (m)');
title('Error Analysis');


% 在MATLAB中，您可以使用 plot 函數來(lái)繪制線(xiàn)條和散點(diǎn)圖，但是不能將多個(gè)數據集的線(xiàn)條和散點(diǎn)圖組合在一起繪制。
% 如果您想繪制兩個(gè)數據集的線(xiàn)條和散點(diǎn)圖，請使用兩個(gè)獨立的 plot 函數調用來(lái)繪制它們
% figure
% hold on; box on;
% plot(Err_Messure_x,'-ko','MarkerFace','g')
% plot(Err_Kalman_x,'-ks','MarkerFace','r')
% legend('messure error','kalman error')


%% 自定義函數，求歐拉距離的： d = sqrt(?x^2 + ?y^2)
function dist=RMS(X1,X2)
  if length(X2)<=2
 ? ? ? ?dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(2))^2 );
 ? ?else
 ? ? ? ?dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(3))^2 );
 ? ?end
end

5.4 結果分析

真實(shí)值、測量值、KF得到的值如下，可以看到明顯的效果。

測量值、KF得到的值與真實(shí)值做差對比如下。