matlab运行RTKDEMO几个小函数

左手的ㄟ右手 2023-02-11 03:20 1阅读 0赞

# 一、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函数详解][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)  
![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70][]

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

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

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 1][]

# 二、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

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 2][]![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 3][]

## 2、nav inav ##

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

inav:导航电文的卫星号；

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 4][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 5][]

# 三、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 #

[matlab_unique]: https://blog.csdn.net/sinat_40282753/article/details/78373532
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70]: https://img-blog.csdnimg.cn/20200521165025568.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw==,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 1]: https://img-blog.csdnimg.cn/20200521161937957.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw==,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 2]: https://img-blog.csdnimg.cn/20200521163700504.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw==,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 3]: https://img-blog.csdnimg.cn/20200521163733413.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw==,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 4]: https://img-blog.csdnimg.cn/20200521165403209.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw==,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 5]: https://img-blog.csdnimg.cn/20200521165419469.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw==,size_16,color_FFFFFF,t_70