Kalman濾波在船舶GPS導(dǎo)航定位系統(tǒng)中的應(yīng)用（含MATLAB仿真）

• 1、原理介紹
• 2、MATLAB仿真代碼
• • Kalman濾波
  • 計算歐氏距離
• 3、仿真結(jié)果
• • figure1
  • figure2

1、原理介紹

全球定位系統(tǒng)（Global Positioning System，GPS）廣泛運(yùn)用于軍事和民用領(lǐng)域。

GPS接收器可以接受衛(wèi)星的信號，算出接受載體的位置和速度。由于民用GPS導(dǎo)航衛(wèi)星人為的加入了干擾，所以需要對GPS的位置和速度信號進(jìn)行濾波。

GPS中人為加入的干擾信號可以看作是GPS的量測噪聲，也就是觀測噪聲 $V(k)$ 。

---

為了簡化模型，假設(shè)船舶直線運(yùn)動，從港口出發(fā)，以港口為坐標(biāo)原點(diǎn)，設(shè)采樣時間為 $T_0$，用 $s(k)$表示船舶在采樣時刻 $k{T}_0$ 處的真實(shí)位置，用 $y(k)$ 表示在時刻 $k{T}_0$ 處GPS的觀測值，則觀測模型為：
$$y(k)=s(k)+v(k)$$
$v(k)$是GPS定位的量測噪聲，假設(shè)其為高斯白噪聲。

加速度 $a(k)$ 由機(jī)動加速度 $u(k)$ （動力系統(tǒng)控制）和隨機(jī)加速度 $w(k)$ （也就是海浪、風(fēng)的影響）兩部分合成，即;
$$a(k)=u(k)+w(k)$$
假設(shè) $w(k)$ 是符合高斯分布的白噪聲。
定義在采樣時刻 $k{T}_0$ 處系統(tǒng)的狀態(tài) $x(k)$ 是船舶的位置和速度，即
$$x(k)=\left[\begin{array}{c}s(k)\\ \dot{s(k)}\end{array}\right]\text{\hspace{1em}(1)}$$
狀態(tài)方程是：
$$\left[\begin{array}{c}s(k)\\ \dot{s}(k)\end{array}\right]=\left[\begin{array}{cc}1& T_0\\ 0& 1\end{array}\right]\left[\begin{array}{c}s(k?1)\\ \dot{s}(k?1)\end{array}\right]+\left[\begin{array}{c}0.5T_0^2\\ T_0\end{array}\right]u(k?1)+\left[\begin{array}{c}0.5T_0^2\\ T_0\end{array}\right]w(k?1)$$
觀測方程為：
$$y(k)=\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{c}s(k)\\ \dot{s}(k)\end{array}\right]+v(k)$$
接著將勻速直線運(yùn)動拓展到四維，即
$$?\begin{array}{c}x(k)\\ \dot{x}(k)\\ y(k)\\ \dot{y}(k)\end{array}?=?\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}??\begin{array}{c}x(k?1)\\ \dot{x}(k?1)\\ y(k?1)\\ \dot{y}(k?1)\end{array}?+?\begin{array}{cc}0.5T_0^2& 0\\ 0& T_0\\ 0.5T_0^2& 0\\ 0& T_0\end{array}?w_{2\times 1}(k)$$
$$Z(k)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]?\begin{array}{c}x(k)\\ \dot{x}(k)\\ y(k)\\ \dot{y}(k)\end{array}?+v_{2\times 1}(k)$$

2、MATLAB仿真代碼

Kalman濾波

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %功能說明：Kalman在GPS中的應(yīng)用 function Kalman_GPS T = 1; %雷達(dá)掃描周期 N = 80/T; %總的采樣次數(shù) X = zeros(4,N); %目標(biāo)真實(shí)位置、速度 X(:,1) = [-100,2,200,20]; %目標(biāo)初始位置和速度 Z = zeros(2,N); %傳感器對位置的量測 Z(:,1) = [X(1,1),X(1,3)]; %觀測初始化 delta_w = 1e-2; %如果增大這個參數(shù)，目標(biāo)真實(shí)軌跡就是曲線了 Q = delta_w*diag([0.5,1,0.5,1]); %過程噪聲方差 R = 100*eye(2); %觀測噪聲方差 F = [1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1] %一步轉(zhuǎn)移陣 H = [1,0,0,0;0,0,1,0] %量測矩陣 I = eye(4); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for k = 2:NX(:,k) = F*X(:,k-1) + sqrtm(Q)*randn(4,1); %目標(biāo)真實(shí)軌跡Z(:,k)= H*X(:,k) + sqrtm(R)*randn(2,1); %目標(biāo)觀測 end%Kalman濾波 X_kf = zeros(4,N); X_kf(:,1) = X(:,1); %Kalman濾波狀態(tài)初始化 P = eye(4); %協(xié)方差陣初始化 for k = 2:NX_pre = F*X_kf(:,k-1); %狀態(tài)預(yù)測P_pre = F*P*F' + Q; %協(xié)方差預(yù)測Kg = P_pre*H'*inv(H*P_pre*H' + R); %計算Kalman增益e = Z(:,k) - H*X_pre; %新息X_kf(:,k) = X_pre + Kg*e; %狀態(tài)更新P = (I-Kg*H)*P_pre; %協(xié)方差更新 end %誤差分析 for i = 1:NError_Observation(i) = RMS(X(:,i),Z(:,i)); Error_Kalman(i) = RMS(X(:,i),X_kf(:,i)); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure %畫圖顯示 hold on; box on; plot(X(1,:),X(3,:),'-k'); %真實(shí)軌跡 plot(Z(1,:),Z(2,:),'-b.'); %觀測軌跡 plot(X_kf(1,:),X_kf(3,:),'-r+'); %kalman濾波軌跡 legend('真實(shí)軌跡','觀測軌跡','Kalman濾波軌跡'); xlabel('X/m'); ylabel('Y/m');figure %誤差分析圖 hold on; box on; plot(Error_Observation,'-ko','MarkerFace','g'); plot(Error_Kalman,'-ks','MarkerFace','r'); legend('濾波前誤差','濾波后誤差'); xlabel('采樣時間/s'); ylabel('誤差值/℃');

計算歐氏距離

%計算歐氏距離 function dist = RMS(X1,X2) if length(X2) <= 2dist=sqrt((X1(1)-X2(1))^2 + (X1(3)-X2(2))^2); elsedist=sqrt((X1(1)-X2(1))^2 + (X1(3)-X2(3))^2); end

3、仿真結(jié)果

figure1

figure2

總結(jié)

以上是生活随笔為你收集整理的Kalman滤波在船舶GPS导航定位系统中的应用（含MATLAB仿真）的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。