gpsmdl.m

来自「自己编写的鲁棒滤波算法的应用」· M 代码 · 共 16 行

function [StM,VtM] = GPSMdl(St,Vt,Pc)
%*****************************************************************
% ***********  给位置信息和速度信息加入测量误差 ******************
% Pc -- 误差协方差阵;    Vt -- 真实速度;      St -- 真实位置;
%*************************************************************常数
% Re -- 地球半径;     m,n -- 矩阵维数
Re  = 6378137;        [m,n] = size(St);
%-------------------------------------------------------------误差
% StE -- 经纬度测量误差;        VtE -- 速度误差;
Pcm = sqrt(Pc);
StE = [Pcm(1)*randn(1,n).*sec(St(2,:))/Re; Pcm(2)*randn(1,n)/Re]; 
VtE = [Pcm(3)*randn(1,n); Pcm(4)*randn(1,n); Pcm(5)*randn(1,n)];
%-----------------------------------------------------------测量值
%  StM -- 经纬度实际测量值;      VtM -- 速度实际测量值
StM = St + StE;                     VtM = Vt + VtE;
%*****************************************************************