matlab运行RTKDEMO几个小函数

一、matlab函数：

1、datenum() :

将时间点转换为0000年1月0日到现在的天数！其中起点为：0000年1月0日

 例如：

K>> t0=datenum(2005,4,2,0,0,0)
t0 =
      732404

K>> t0=datenum(2005,4,2,12,0,0)
t0 =
                  732404.5

K>> t0=datenum(2005,4,2)
t0 =
      732404

2、unique():

用法1：[C,ia,ic]=unique(A,'rows');

Define a matrix with a repeated row.

A = [9 2 9 5; 
     9 2 9 0; 
     9 2 9 5];
Find the unique rows of A and the index vectors ia and ic, such that C = A(ia,:) and A = C(ic,:).
%按照‘行’顺序，进行排序，
% 1、两行作比较，如两行前几个为相同的元素，从左到右依次比较；
% 2、结果按升序排列；
[C, ia, ic] = unique(A,'rows')
C =

     9     2     9     0
     9     2     9     5
% ia为 矩阵C中元素在矩阵A中的位置
ia =

     2
     1
% ic为 矩阵A中元素在矩阵C中的位置
ic =

     2
     1
     2

其中在代码实现时，用法如下：

• 按照‘rows’/行 ，升序排序；
• n为升序后的矩阵，i为矩阵n中元素("行"为一个元素)在矩阵nav(:,1:33)中的位置；
• 如果有同样的元素，则删去；

[n,i]=unique(nav(:,1:33),'rows'); 
nav=nav(i,:); 
inav=inav(i,:);

参考：matlab中的unique函数详解

3、sortrows():

用法1：[B,i]=sortrows(A)

表示：对矩阵A中的行元数进行大小比较，如果每行中第一列元素相等，则比较第二列……/从左向右，一次比下去；

i为排序后的矩阵B中行元素在矩阵A中的位置：

示例如下：

 A =[95    45    92    41    13     1    84
    95    45    92    41    13     1    84
    95     7    73     5    19    44    20
    95     7    40    35    60    93    67
    76    61    93    81    27    46    83
    76    79    91     0    19    41     1];

[B,i]=sortrows(A)

在代码中，其实现和功能为：将移动站和基准站相同历元观测到的相同卫星进行1、2标识。

[iobs,i]=sortrows(iobs); obs=obs(i,:);

二、readrnx 函数：

% read RINEX observation data/navigation messages  观测数据 导航电文
[obs,iobs,nav,inav]=readrnx(t0,time(1),time(end),files); % iobs中 rover:1,ref.:2

读取文件依次为：流动站观测文件、基准站观测文件；流动站导航文件、基准站导航文件；

输出矩阵：

1、obs iobs

obs为按照数据类型：'L1','L2','C1','P2'；输出的数据；

其中第一行为流动站第一个历元数据；第二行为基准站第一个历元数据；

iobs：第一列为历元时间，第二列为卫星号，第三列为：1/rover和2/base

2、nav inav

nav: 基准站和移动站导航电文，经过unique处理后（即：按照时间的升序），删除重复的导航电文信息；

inav:导航电文的卫星号；

三、rtkdemo()

[xp,xs,xf]=rtkdemo(t0,time,rref,obs,iobs,nav,inav,mode)

函数说明/处理策略：

• 取第一个历元，高度角最大的卫星为参考卫星；
• 利用L1频点的双差载波对EKF状态量进行更新；
• 通过LAMBDA/MLAMBDA方法求解模糊度；
• 对于周跳和粗差不处理；

1、流动站单点定位计算：

计算出流动站坐标、高度角最大卫星；

[rr,t,sat,dop]=pointp(t0,obsk,iobsk,nav,inav);

2、EKF的时间更新：

[x,P]=udstate(sref,sats,x,P,rr(:,1),obsk,iobsk,mode,lam);

状态量：单点定位坐标、双差模糊度(载波双差-伪距双差/lam)/cycle

其中，双差模糊度噪声设置为：10厘米

状态协方差矩阵P为对角阵，且P=Q.^2

3、L1频段载波双差测量值、双差预测值：

（1）L1频段双差载波测量值：y为载波相位双差值！

y=lam*obs_dd(sref,sats,obsk,iobsk,1);%L1双差,单位：米/m

（2）测量模型：

h为伪距双差值+模糊度(单位：mi)；其中，移动站坐标是每一个历元通过单点定位得到。即双差预测值

H为设计矩阵；

R为模糊度噪声；单位：m/米

[h,H,R]=measmodel(t0,t(1),t(2),x(1:3),rref,x(4:end),sref,sats,nav,inav,lam);

4、kalman测量更新：

% measurement update of states -------------------------------------------------
function [x,P]=filt(x,P,y,h,H,R)
i=find(~isnan(y)&~isnan(h)); H=H(i,:);
K=P*H'/(H*P*H'+R(i,i));
x=x+K*(y(i)-h(i));
P=P-K*H*P;

5、模糊度固定：

 

% ambiguity resolution ---------------------------------------------------------
function x=fixamb(x,P);
i=1:3; j=4:length(x); 
j=j(~isnan(x(j))&diag(P(j,j))<10^2);
[N,s]=mlambda(x(j),P(j,j),2);
if isempty(N)|s(2)/s(1)<3, x(:)=nan; return, end  % ratio-test
x([i,j])=[x(i)-P(i,j)/P(j,j)*(x(j)-N(:,1));N(:,1)];

 

四、plot_pos