一文詳解卡爾曼濾波

作者：嗣音 | 來源：古月居

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

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

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

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

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

一、線性高斯系統(tǒng)

1.1 線性系統(tǒng)

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

所謂線性系統(tǒng)即指滿足疊加性與齊次性的系統(tǒng),反之即為非線性系統(tǒng)。

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

線性系統(tǒng)齊次原理：當(dāng)輸入信號放大 k 倍時，輸出信號也會放大 k 倍。

平時可以根據(jù)這幾點(diǎn)判斷線性or非線性

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

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

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

舉個例子，2x+3y=5 是一個線性方程，它只包含了一次項(xiàng)；而 x^2+y^2=4 是一個非線性方程，因?yàn)樗似椒巾?xiàng)；xy=2 是一個非線性方程，因?yàn)樗藘蓚€變量的乘積。

1.2 高斯分布

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

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

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

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

matlab代碼：

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


% 定義不同的高斯分布的均值和標(biāo)準(zhǔn)差
mu1 = 0;
sigma1 = 1;


mu2 = 2;
sigma2 = 0.8;


mu3 = -2;
sigma3 = 1.5;


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


% 繪制高斯分布曲線
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');


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

1.3 高維高斯分布

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

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

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

matlab代碼：

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


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


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


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


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


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


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


% 繪制高斯分布曲線
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');


% 添加圖例和標(biāo)簽
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 線性高斯系統(tǒng)

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

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

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

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

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

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

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

二、卡爾曼濾波算法

2.1 算法理解

其推導(dǎo)這里不做記錄，可以參照《概率機(jī)器人》

使用過程就是這五個黃金公式一直套就對了

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

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

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

方差是通過線性矩陣A(t)考慮了當(dāng)前狀態(tài)依賴前一狀態(tài)的的事實(shí)進(jìn)行計算的。因?yàn)榉讲钍且粋€二次型的，所以該矩陣兩次相乘得到方差。

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

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

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

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

2.2 特點(diǎn)描述

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

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

2.Kalman濾波算法是一種時間域?yàn)V波的方法，采用狀態(tài)空間描述系統(tǒng)。

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

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

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

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

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

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

2.3 噪聲矩陣的處理

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

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

如何確定 R 和 Q 呢？

1.對于過程噪聲方差 R

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

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

要準(zhǔn)確獲取過程噪聲方差 R 比較困難，可以通過對比試驗(yàn)獲得。

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

2.對于觀測噪聲方差 Q

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

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

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

一般地，觀測噪聲的方差 Q 是一個統(tǒng)計意義上的參數(shù)：對傳感器測量的數(shù)據(jù)經(jīng)過長期的概率統(tǒng)計，得到它測量噪聲的方差。

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

3.1 問題描述

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

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

這個實(shí)例的代表意義是，他是一個一維的線性高斯系統(tǒng)問題。

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

過程噪聲是均值為0、方差 R=0.01的高斯分布；其狀態(tài)轉(zhuǎn)移方程：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)品說明上得知其方差為0.25，也就是說有測量噪聲。

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

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

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

3.2 手動解算

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

假定第 t 時刻溫度測量值為24.5℃

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

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

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

計算卡爾曼增益

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

則此時利用 t 時刻的觀測值，得到溫度計的估值為:

均值 μ(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

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

3.3 matlab實(shí)現(xiàn)

%% 根據(jù)系統(tǒng)描述，初始化一些值
clc;clear;
N = 300;          % 采樣次數(shù)


X = zeros(1,N);       % 狀態(tài)值 真值
Z = zeros(1,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)轉(zhuǎn)移方程相關(guān)
C = 1;           % 測量方程相關(guān)
I = eye(1);         % 單位矩陣


R = 0.01;          % 過程噪聲方差
Q = 0.25;          % 測量噪聲方差


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




%% 根據(jù)噪聲，模擬實(shí)際數(shù)據(jù) 和 測量數(shù)據(jù)
W = sqrt(R)*randn(1,N);   % 過程噪聲 ε(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);          % 預(yù)測狀態(tài)均值 _μ(t)
  P_pre = A*P_kf(t-1) + R;        % 預(yù)測狀態(tài)方差 _Σ(t)
  K = P_pre/(P_pre + Q);         % 更新卡爾曼增益 K
  X_kf(t) = X_pre + K*(Z(t) - C*X_pre);  % 更新狀態(tài)均值 μ(t)
  P_kf(t) = (I - K*C)*P_pre;        % 更新狀態(tài)方差 Σ(t)
end




%% 分析誤差項(xiàng)
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




%% 畫圖
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 結(jié)果分析

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

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

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

四、實(shí)例二：自由落體運(yùn)動目標(biāo)跟蹤

4.1 問題描述

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

需要估計該物體的運(yùn)動位移 x 和速度 v。

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

2.根據(jù)運(yùn)動學(xué)方程，該物體的狀態(tài)轉(zhuǎn)移方程為:

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

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

4.2 手動解算

略，看matlab代碼及注釋。

4.3 matlab實(shí)現(xiàn)

printf("hello world!");

4.4 結(jié)果分析

真實(shí)值、測量值、KF得到的值如下，因?yàn)橹当容^大，所以這里不是很能看出什么。

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

五、實(shí)例三：船舶GPS導(dǎo)航定位系統(tǒng)

5.1 問題描述

1.船舶出港沿著某直線方向航行，采樣間隔?t。

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

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

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

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

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

3. 給定GPS觀測裝置觀測位置，民用GPS導(dǎo)航衛(wèi)星播放的信號人為加入了高頻震蕩隨機(jī)干擾信號。

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

觀測方程寫為：

5.2 手動解算

略，看matlab代碼及注釋。

5.3 matlab實(shí)現(xiàn)

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




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




A =[1,T,0,0;          % 狀態(tài)轉(zhuǎn)移方程相關(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);




%% 根據(jù)噪聲，模擬實(shí)際數(shù)據(jù) 和 測量數(shù)據(jù)
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




%% 分析誤差項(xiàng)
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：自定義函數(shù)，求歐拉距離的
  Err_Kalman_x(t) = RMS(X(:,t),X_kf(:,t));
end




%% 畫圖
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 函數(shù)來繪制線條和散點(diǎn)圖，但是不能將多個數(shù)據(jù)集的線條和散點(diǎn)圖組合在一起繪制。
% 如果您想繪制兩個數(shù)據(jù)集的線條和散點(diǎn)圖，請使用兩個獨(dú)立的 plot 函數(shù)調(diào)用來繪制它們
% figure
% hold on; box on;
% plot(Err_Messure_x,'-ko','MarkerFace','g')
% plot(Err_Kalman_x,'-ks','MarkerFace','r')
% legend('messure error','kalman error')


%% 自定義函數(shù)，求歐拉距離的： 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 結(jié)果分析

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

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