船舶GPS導(dǎo)航定位原理如圖

所示，將一臺(tái) GPS接收機(jī)安裝在運(yùn)動(dòng)目標(biāo)（船舶）上就可以進(jìn)行導(dǎo)航定位計(jì)算。GPS 接收機(jī)可以實(shí)時(shí)收到在軌的導(dǎo)航衛(wèi)星播發(fā)的信號(hào)，計(jì)算出接收載體（船舶）的位置和速度。由于民用領(lǐng)域CPS導(dǎo)航衛(wèi)星播發(fā)的信號(hào)人為加入了高頻振蕩隨機(jī)干擾信號(hào)，致使所有派生的衛(wèi)星信號(hào)均產(chǎn)生高頻抖動(dòng)。為了提高定位精度，需要對(duì)GPS關(guān)于船舶的位置和速度的觀測(cè)信號(hào)進(jìn)行濾波。在 GPS系統(tǒng)中人為加入的高頻隨機(jī)干擾信號(hào)可看成觀測(cè)噪聲、觀測(cè)噪聲強(qiáng)度（方差）可用系統(tǒng)辨識(shí)方法求得。
為將模型簡(jiǎn)單化.假定船舶出港沿某直線方向航行。以港口碼頭的出發(fā)處為坐標(biāo)原點(diǎn)，設(shè)采樣時(shí)間為T，用s（k）表示船舶在采樣時(shí)刻T。處的真實(shí)位置，用y（k）表示在時(shí)刻kT。處
GPS定位的觀測(cè)值，則有觀測(cè)模型∶
$$y(k)=s(k)+v(k)$$

%%%%%%%%%%%%%%%%%%%%%%%%%% %% %%%%%%%%%%%%%%% %% function KalmanForGPS % Kalman濾波在船舶GPS導(dǎo)航定位系統(tǒng)中的應(yīng)用 dt = 1; %雷達(dá)掃描周期T=80/ dt; %總的采樣次數(shù)F=[ 1, dt ,0 ,0;0,1,0,0;0,0, 1 , dt ;0 ,0,0,1 ] ; %狀態(tài)轉(zhuǎn)移矩陣H=[ 1,0,0,0;0,0,1,0]; %。觀測(cè)矩陣delta_w= 1e-2; % 如果增大這個(gè)參數(shù),目標(biāo)真實(shí)軌跡就是曲線了Q=delta_w * diag( [ 0.5,1 ,0.5,1]) ; %過程噪聲方差R= 10 * eye( 2); %觀測(cè)噪聲方差W = sqrtm( Q) * randn( 4,T); %過程噪聲V =sqrtm(R) * randn( 2,T); %觀測(cè)噪聲%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%% %%%%%%%%%% X = zeros( 4,T); %。目標(biāo)真實(shí)位置、速度X( :,1)=[-100,2,200, 20]; %目標(biāo)初始位置、速度Z=zeros( 2,T); %傳感器對(duì)位置的觀測(cè)Z( :,1)=[X( 1,1 ),X(3,1)]; %觀測(cè)初始化Xkf=zeros( 4,T); %。Kalman濾波狀態(tài)初始化Xkf( : ,1)=X( :,1 ); P=eye( 4 ); %協(xié)方差陣初始化for k=2:T%船體自身運(yùn)動(dòng)X ( : ,k)=F* X( :,k- 1)+W( :,k ) ; %目標(biāo)真實(shí)軌跡%獲取衛(wèi)星數(shù)據(jù),觀測(cè)信息開始濾波Z( :,k)=H*X( :,k)+V( :,k); %自導(dǎo)航,觀測(cè)信息%% PredictXpre =F* Xkf( : ,k-1 ); %第一步:狀態(tài)預(yù)測(cè)Ppre=F*P*F'+Q; %第二步:協(xié)方差預(yù)測(cè)%% UpdateK=Ppre*H'*inv( H * Ppre * H'+R); %第三步:求增益Xkf( : ,k )= Xpre+K*(Z( :,k)-H * Xpre);%第四步:狀態(tài)更新P=(eye( 4)-K* H) * Ppre ; %第五步:協(xié)方差更新end %誤差分析 for i= 1 :TErr_Observation( i)= RMS(X( :,i) ,Z( : ,i)); %濾波前的誤差Err_KalmanFilter( i)= RMS(X( :,i) ,Xkf( : ,i) );%濾波后的誤差end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %畫圖 figure %軌跡圖 hold on ; box on ; xlabel ( 'X/m') ; ylabel( 'Y/m' ) ; plot( X( 1 ,: ) ,X( 3,: ) ,'-k'); %真實(shí)軌跡plot( Z( 1 , :) ,Z( 2,: ) , '-b. ' ); %觀測(cè)軌跡plot ( Xkf(1 ,: ),Xkf( 3, : ) , '-r+'); %Kalman 濾波軌跡legend('真實(shí)軌跡','觀測(cè)軌跡','濾波軌跡');figure %。誤差圖hold on; box on ; xlabel( 'Time/s' ); ylabel( 'value of the deviation/ m') ; plot( Err_Observation , '-ko' , 'MarkerFace' , 'g'); plot( Err_KalmanFilter , '-ks' , 'MarkerFace' , 'r'); legend('觀測(cè)偏差','濾波后偏差') % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%% %% %% %子函數(shù):計(jì)算偏差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 );endend %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% end```

總結(jié)

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

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