calc_float_solution.m该文件负责 raPPPid 单个历元的浮点参数估计。它把观测建模、设计矩阵、随机模型和估计算法串联起来。运行流程输入当前历元和上一历元状态↓初始化模型结构体↓检查并计算概略位置↓检查并计算概略速度↓首次卡尔曼滤波前执行LSQ初始化↓进入历元内迭代↓计算卫星位置及全部误差改正↓提取接收机钟差/系统间偏差↓计算几何距离↓初始化待估电离层↓生成码、相位和多普勒理论值↓进行OMC粗差检测↓DCM模式选择可固定卫星和参考卫星↓DSM_ZD或DSM_DCM建立设计矩阵↓建立观测协方差矩阵↓检查有效观测数量↓最小二乘或卡尔曼滤波↓判断收敛↓保存浮点解和协方差↓重置剔除卫星模糊度↓重置剔除卫星电离层↓周跳后重置模糊度一、定义变量读取是否处于 parfor 并行模式并取逻辑非读取实际处理频率数读取基础估计参数数量 NO_PARAM。后续用它定位模糊度参数在状态向量中的起始位置numel 返回 Epoch.sats 中的元素总数即当前历元卫星数读取参数估计方法文本创建结构体字段 dx.x并用 zeros(size(...))生成与 Adjust.param 同尺寸的全零增量向量把本历元内部迭代次数初始化为0。% Preparations bool_print ~settings.INPUT.bool_parfor; % print to command window? num_freq settings.INPUT.proc_freqs; % number of processed frequencies NO_PARAM Adjust.NO_PARAM; % number of estimated parameters no_sats numel(Epoch.sats); % total number of satellites in current epoch filter_type settings.ADJ.filter.type; % method of parameter estimation dx.x zeros(size(Adjust.param)); % initialize it 0; % number of current iteration二、初始化结构模型% Initialize struct model model init_struct_model(no_sats, settings.INPUT.proc_freqs, settings.INPUT.num_freqs);三、计算概略位置% vector (e.g., dynamic model is zero (no model)) if any(Adjust.param(1:3) 0) || any(Adjust.param_pred(1:3) 0) xyz_ ApproximatePosition(Epoch, input, obs, settings); if any(Adjust.param(1:3) 0); Adjust.param(1:3) xyz_; end if any(Adjust.param_pred(1:3) 0); Adjust.param_pred(1:3) xyz_; end end四、计算概略速度if any(Adjust.param(4:6) 0) || any(Adjust.param_pred(4:6) 0) [vel_, ~] ApproximateVelocity(Epoch, input, obs, settings, Adjust.param(1:3)); if any(Adjust.param(4:6) 0); Adjust.param(4:6) vel_; end if any(Adjust.param_pred(4:6) 0); Adjust.param_pred(4:6) vel_; end end五、正式卡尔曼滤波前先做一次最小二乘满足以下三个条件才执行1当前还没有有效浮点解2用户选择普通卡尔曼滤波3当前不是卫星定轨模式。% make sure that Kalman Filter has good approximate initial parameters (e.g., huge receiver clock error) if ~Adjust.float strcmp(filter_type, Kalman Filter) ~settings.KINE.satellite.bool1. 构造临时最小二乘设置LSQ_setts settings; LSQ_setts.ADJ.filter.type No Filter; LSQ_setts.INPUT.bool_parfor true;2.递归调用本函数[~, LSQ, ~] calc_float_solution( ... input, obs, Adjust, Epoch, LSQ_setts);3. 用LSQ结果替换卡尔曼初值Adjust.param(1:NO_PARAM) LSQ.param(1:NO_PARAM); Adjust.param_pred(1:NO_PARAM) LSQ.param(1:NO_PARAM);4. 将残余对流层湿延迟重新设为零bool_zwd strcmp(Adjust.ORDER_PARAM, zwd); Adjust.param(bool_zwd) 0; Adjust.param_pred(bool_zwd) 0;六、进入历元内迭代while it DEF.ITERATION_MAX_NUMBER it it 1;迭代过程使用当前参数计算理论观测值↓建立A矩阵和OMC↓计算参数改正量dx↓更新参数↓重新计算理论观测值↓判断坐标改正是否足够小七、判断是否重新进行完整误差建模coord_jump it 2 norm(dx.x(1:3)) 0.05; clock_jump it 2 ... MajorReceiverClockChange(dx.x, settings.IONO.model);1. 坐标变化超过5 cmnorm(dx.x(1:3)) 0.05如果本次迭代坐标变化超过0.05 m就需要重新计算几何距离卫星高度角和方位角对流层映射函数潮汐改正天线相位中心地球自转改正相关方向性误差。2. 接收机钟差发生明显变化MajorReceiverClockChange(...)3. 调用核心误差建模函数modelErrorSourcesif it 1 || coord_jump || clock_jump [model, Epoch] modelErrorSources( ... settings, input, Epoch, model, Adjust, obs); end八、计算接收机钟差和系统间偏差model getReceiverClockBiases( ... model, Epoch, Adjust.param_pred, settings);该函数把状态向量中的接收机钟参数映射到每颗卫星和每个频率。九、重新计算视线距离和几何距离los vecnorm2(model.Rot_X - Adjust.param_pred(1:3)); model.rho repmat(los, 1, num_freq);十、初始化待估电离层参数if strcmpi(settings.IONO.model, ... Estimate with ... as constraint) || ... strcmpi(settings.IONO.model, Estimate)只有使用非组合模型并显式估计电离层时才进入这里。1. 定位电离层参数n numel(Adjust.param); idx_iono n-no_sats1:n;2. 判断哪些电离层参数尚未初始化iono_0 (Adjust.param(idx_iono) 0);3.使用模型电离层值初始化Adjust.param(idx_iono(iono_0)) ... model.iono(iono_0,1); Adjust.param_pred(idx_iono(iono_0)) ... model.iono(iono_0,1);十一、生成理论伪距、相位和多普勒[model.model_code, ... model.model_phase, ... model.model_doppler] ... model_observations(model, Adjust, settings, Epoch);十二、观测值减计算值粗差检测if settings.PROC.check_omc [Epoch, Adjust] check_omc( ... Epoch, model, Adjust, settings, obs.interval); end十三、解耦时钟模型参考卫星选择if strcmp(settings.IONO.model, ... Estimate, decoupled clock) Epoch CheckSatellitesFixable( ... Epoch, settings, model, input); [Epoch, Adjust] handleRefSats( ... Epoch, model.el, settings, Adjust); end十四、建立设计矩阵和OMC向量switch settings.PROC.method程序根据处理方式选择不同的设计矩阵构建函数。1. 码加相位或码相位多普勒case {Code Phase, ... Code Phase Doppler}普通非差模型[A, omc] DSM_ZD( ... Adjust, Epoch, model, settings);解耦时钟模型[A, omc] DSM_DCM( ... Adjust, Epoch, model, settings);2. 仅使用码或码加多普勒case {Code Doppler, ... Code Only, ... Code (Doppler Smoothing), ... Code (Phase Smoothing)}3. 增加多普勒观测方程if contains(settings.PROC.method, Doppler) [A, omc] DSM_add_Doppler( ... A, omc, Adjust, Epoch, model, settings); end4. 保存矩阵Adjust.A A; Adjust.omc omc;之后最小二乘或卡尔曼滤波直接使用这两个矩阵。十五、构建观测协方差矩阵Adjust createObsCovariance( ... Adjust, Epoch, settings, model.el, model.bore);十六、检查有效观测是否足够n_observations numel(Epoch.exclude);1. 统计被排除观测n_reject sum( ... Epoch.exclude(:) | ... Epoch.cs_found(:) * ... strcmp(settings.IONO.model, GRAPHIC));2.判断剩余观测数量if n_observations - n_reject DEF.MIN_SATS3. 观测不足时退出dx.x(1:3) 0; Adjust.res NaN(numel(Adjust.omc),1); Adjust.float false; Adjust.fixed false; return含义当前历元不给坐标改正残差设为NaN浮点解无效固定解也无效直接结束当前历元十七、历元内迭代卡尔曼滤波case Kalman Filter Iterative1. 首次迭代初始化if it 1 x_pred zeros(size(Adjust.A,2),1); end2. 执行迭代卡尔曼滤波dx KalmanFilterIterative(Adjust, x_pred);3. 累积线性化改正x_pred x_pred - dx.x;4. 判断坐标是否收敛if norm(dx.x(1:3)) DEF.ITERATION_THRESHOLD若坐标改正小于阈值调用Adjust stop_iteration(Adjust, dx); break;5. 未收敛则更新参数Adjust.param Adjust.param dx.x; Adjust.param_pred Adjust.param; Adjust.param_sigma_pred Adjust.param_sigma;十八、普通卡尔曼滤波case Kalman Filter调用[Adjust.param, ... Adjust.param_sigma, ... Adjust.float] ... KalmanFilter( ... Adjust.omc, ... Adjust.A, ... Adjust.param_pred, ... Adjust.param_sigma_pred, ... Adjust.Q);1. 状态更新公式2. 重新计算验后残差[Adjust.res, Adjust.res_doppler] ... calc_res(settings, input, Epoch, model, Adjust, obs);由于卡尔曼滤波更新了状态参数原来的 OMC 是基于预测状态得到的不能直接当作验后残差。所以需要用更新后状态重新建模重新计算理论观测值再计算观测减理论值。3. 普通卡尔曼滤波不进行历元内迭代即每个历元只做一次滤波更新。因此对初值要求更高这也是前面要先执行一次LSQ初始化的原因。十九、单历元最小二乘case No Filter调用dx adjustment(Adjust);1. 判断收敛if norm(dx.x(1:3)) DEF.ITERATION_THRESHOLD坐标改正足够小时Adjust stop_iteration(Adjust, dx); break;2. 未收敛继续迭代Adjust.param Adjust.param dx.x; Adjust.param_pred Adjust.param; Adjust.param_sigma_pred Adjust.param_sigma;更新线性化点后继续计算。二十、判断当前历元是否最终收敛if ~strcmp(filter_type, Kalman Filter) ... norm(dx.x(1:3)) DEF.ITERATION_THRESHOLD普通卡尔曼滤波不进入该检查因为它本来就不进行历元内迭代。对于No FilterKalman Filter Iterative如果达到最大迭代次数后坐标改正仍大于阈值则认为不收敛。不收敛处理Adjust.float false; Adjust.res NaN(numel(Adjust.omc),1);表示当前浮点解无效。注意这里没有将Adjust.param恢复为迭代前状态。因此后续函数是否仍使用这组参数需要检查主程序的无效解处理逻辑。二十一、重置被排除卫星的模糊度if contains(settings.PROC.method, Phase) ... any(Epoch.exclude(:))1. 给所有卫星频率组合编号kk 1:(num_freq*no_sats);2. 找出被排除观测的编号kk kk(Epoch.exclude(:));3. 转换成状态向量中的模糊度下标idx_amb kk NO_PARAM;4. 重置参数和协方差Adjust reset_param_sigma( ... Adjust, idx_amb, settings.ADJ.filter.var_amb);二十二、重置被排除卫星的电离层参数if contains(settings.IONO.model, Estimate) ... any(Epoch.exclude(:,1))只检查第一频率是否被排除1. 获取卫星编号kkk 1:100; kkk kkk(Epoch.exclude(:,1));2. 计算电离层状态位置idx_iono kkk NO_PARAM;如果同时处理相位模糊度位于电离层参数之前if contains(settings.PROC.method, Phase) idx_iono idx_iono num_freq*no_sats; end3. 重置电离层参数Adjust reset_param_sigma( ... Adjust, idx_iono, settings.ADJ.filter.var_iono);使该卫星重新进入时可以重新初始化电离层而不是沿用失锁前的电离层状态。二十三、周跳后重置模糊度if contains(settings.PROC.method, Phase) ... any(Epoch.cs_found(:))只要检测到周跳就将对应模糊度状态重置。1. 观测编号kkkk 1:410; kkkk kkkk(Epoch.cs_found(:));2. 定位模糊度状态idx_cs kkkk NO_PARAM;3. 重置状态Adjust reset_param_sigma( ... Adjust, idx_cs, settings.ADJ.filter.var_amb);二十四、辅助函数一判断接收机钟差是否发生大变化function bool MajorReceiverClockChange(dx, iono_model)普通模型sum_rec_clk ... dx(8) dx(11) dx(14) dx(17) dx(20);解耦时钟模型if strcmp(iono_model, Estimate, decoupled clock) sum_rec_clk ... dx(8) dx(9) dx(10) dx(11) dx(12); end二十五、辅助函数二停止迭代并保存结果function Adjust stop_iteration(Adjust, dx)1. 标记浮点解有效Adjust.float true;2. 保存估计参数Adjust.param Adjust.param dx.x;3. 保存验后残差Adjust.res dx.v;4. 保存参数协方差Adjust.param_sigma dx.Qxx;二十六、辅助函数三重置参数和协方差function Adjust reset_param_sigma( ... Adjust, idx, initial_var)1. 参数置零Adjust.param(idx) 0;2. 清除与其他参数的相关性Adjust.param_sigma(idx,:) 0; Adjust.param_sigma(:,idx) 0;3. 重设对角方差sz size(Adjust.param_sigma); idx_ sub2ind(sz, idx, idx); Adjust.param_sigma(idx_) initial_var;二十七、辅助函数四卡尔曼滤波后重新计算残差function [res, res_doppler] ... calc_res(settings, input, Epoch, model, Adjust, obs)1. 使用滤波后的参数重新建模Adjust.param_pred Adjust.param;2. 重新计算几何距离los vecnorm2( ... model.Rot_X - Adjust.param(1:3)); model.rho repmat( ... los, 1, settings.INPUT.proc_freqs);3. 重新计算接收机钟差model getReceiverClockBiases( ... model, Epoch, Adjust.param, settings);4. 重新计算误差项和理论观测[model, Epoch] modelErrorSources(...); [code_model, phase_model, doppler_model] ... model_observations(...);二十八、码相位残差的排列方式s_f numel(Epoch.sats) * ... settings.INPUT.proc_freqs;1. 初始化交替排列的残差向量res zeros(s_f, 1); code_row 1:2:2*s_f; phase_row 2:2:2*s_f;2. 码残差res(code_row,:) ... (Epoch.code(:) - code_model(:)) .* ~exclude;3. 相位残差usePhase ~Epoch.cs_found(:); res(phase_row,:) ... (Epoch.phase(:) - phase_model(:)) .* ... ~exclude .* usePhase;4. GRAPHIC模式if strcmp(settings.IONO.model, GRAPHIC) res(code_row,:) []; end二十九、多普勒残差if contains(settings.PROC.method, Doppler)计算res_doppler ... (Epoch.doppler(:) doppler_model(:)) .* ~exclude;
raPPPid之calc_float_solution.m
发布时间：2026/7/24 3:25:43
calc_float_solution.m该文件负责 raPPPid 单个历元的浮点参数估计。它把观测建模、设计矩阵、随机模型和估计算法串联起来。运行流程输入当前历元和上一历元状态↓初始化模型结构体↓检查并计算概略位置↓检查并计算概略速度↓首次卡尔曼滤波前执行LSQ初始化↓进入历元内迭代↓计算卫星位置及全部误差改正↓提取接收机钟差/系统间偏差↓计算几何距离↓初始化待估电离层↓生成码、相位和多普勒理论值↓进行OMC粗差检测↓DCM模式选择可固定卫星和参考卫星↓DSM_ZD或DSM_DCM建立设计矩阵↓建立观测协方差矩阵↓检查有效观测数量↓最小二乘或卡尔曼滤波↓判断收敛↓保存浮点解和协方差↓重置剔除卫星模糊度↓重置剔除卫星电离层↓周跳后重置模糊度一、定义变量读取是否处于 parfor 并行模式并取逻辑非读取实际处理频率数读取基础估计参数数量 NO_PARAM。后续用它定位模糊度参数在状态向量中的起始位置numel 返回 Epoch.sats 中的元素总数即当前历元卫星数读取参数估计方法文本创建结构体字段 dx.x并用 zeros(size(...))生成与 Adjust.param 同尺寸的全零增量向量把本历元内部迭代次数初始化为0。% Preparations bool_print ~settings.INPUT.bool_parfor; % print to command window? num_freq settings.INPUT.proc_freqs; % number of processed frequencies NO_PARAM Adjust.NO_PARAM; % number of estimated parameters no_sats numel(Epoch.sats); % total number of satellites in current epoch filter_type settings.ADJ.filter.type; % method of parameter estimation dx.x zeros(size(Adjust.param)); % initialize it 0; % number of current iteration二、初始化结构模型% Initialize struct model model init_struct_model(no_sats, settings.INPUT.proc_freqs, settings.INPUT.num_freqs);三、计算概略位置% vector (e.g., dynamic model is zero (no model)) if any(Adjust.param(1:3) 0) || any(Adjust.param_pred(1:3) 0) xyz_ ApproximatePosition(Epoch, input, obs, settings); if any(Adjust.param(1:3) 0); Adjust.param(1:3) xyz_; end if any(Adjust.param_pred(1:3) 0); Adjust.param_pred(1:3) xyz_; end end四、计算概略速度if any(Adjust.param(4:6) 0) || any(Adjust.param_pred(4:6) 0) [vel_, ~] ApproximateVelocity(Epoch, input, obs, settings, Adjust.param(1:3)); if any(Adjust.param(4:6) 0); Adjust.param(4:6) vel_; end if any(Adjust.param_pred(4:6) 0); Adjust.param_pred(4:6) vel_; end end五、正式卡尔曼滤波前先做一次最小二乘满足以下三个条件才执行1当前还没有有效浮点解2用户选择普通卡尔曼滤波3当前不是卫星定轨模式。% make sure that Kalman Filter has good approximate initial parameters (e.g., huge receiver clock error) if ~Adjust.float strcmp(filter_type, Kalman Filter) ~settings.KINE.satellite.bool1. 构造临时最小二乘设置LSQ_setts settings; LSQ_setts.ADJ.filter.type No Filter; LSQ_setts.INPUT.bool_parfor true;2.递归调用本函数[~, LSQ, ~] calc_float_solution( ... input, obs, Adjust, Epoch, LSQ_setts);3. 用LSQ结果替换卡尔曼初值Adjust.param(1:NO_PARAM) LSQ.param(1:NO_PARAM); Adjust.param_pred(1:NO_PARAM) LSQ.param(1:NO_PARAM);4. 将残余对流层湿延迟重新设为零bool_zwd strcmp(Adjust.ORDER_PARAM, zwd); Adjust.param(bool_zwd) 0; Adjust.param_pred(bool_zwd) 0;六、进入历元内迭代while it DEF.ITERATION_MAX_NUMBER it it 1;迭代过程使用当前参数计算理论观测值↓建立A矩阵和OMC↓计算参数改正量dx↓更新参数↓重新计算理论观测值↓判断坐标改正是否足够小七、判断是否重新进行完整误差建模coord_jump it 2 norm(dx.x(1:3)) 0.05; clock_jump it 2 ... MajorReceiverClockChange(dx.x, settings.IONO.model);1. 坐标变化超过5 cmnorm(dx.x(1:3)) 0.05如果本次迭代坐标变化超过0.05 m就需要重新计算几何距离卫星高度角和方位角对流层映射函数潮汐改正天线相位中心地球自转改正相关方向性误差。2. 接收机钟差发生明显变化MajorReceiverClockChange(...)3. 调用核心误差建模函数modelErrorSourcesif it 1 || coord_jump || clock_jump [model, Epoch] modelErrorSources( ... settings, input, Epoch, model, Adjust, obs); end八、计算接收机钟差和系统间偏差model getReceiverClockBiases( ... model, Epoch, Adjust.param_pred, settings);该函数把状态向量中的接收机钟参数映射到每颗卫星和每个频率。九、重新计算视线距离和几何距离los vecnorm2(model.Rot_X - Adjust.param_pred(1:3)); model.rho repmat(los, 1, num_freq);十、初始化待估电离层参数if strcmpi(settings.IONO.model, ... Estimate with ... as constraint) || ... strcmpi(settings.IONO.model, Estimate)只有使用非组合模型并显式估计电离层时才进入这里。1. 定位电离层参数n numel(Adjust.param); idx_iono n-no_sats1:n;2. 判断哪些电离层参数尚未初始化iono_0 (Adjust.param(idx_iono) 0);3.使用模型电离层值初始化Adjust.param(idx_iono(iono_0)) ... model.iono(iono_0,1); Adjust.param_pred(idx_iono(iono_0)) ... model.iono(iono_0,1);十一、生成理论伪距、相位和多普勒[model.model_code, ... model.model_phase, ... model.model_doppler] ... model_observations(model, Adjust, settings, Epoch);十二、观测值减计算值粗差检测if settings.PROC.check_omc [Epoch, Adjust] check_omc( ... Epoch, model, Adjust, settings, obs.interval); end十三、解耦时钟模型参考卫星选择if strcmp(settings.IONO.model, ... Estimate, decoupled clock) Epoch CheckSatellitesFixable( ... Epoch, settings, model, input); [Epoch, Adjust] handleRefSats( ... Epoch, model.el, settings, Adjust); end十四、建立设计矩阵和OMC向量switch settings.PROC.method程序根据处理方式选择不同的设计矩阵构建函数。1. 码加相位或码相位多普勒case {Code Phase, ... Code Phase Doppler}普通非差模型[A, omc] DSM_ZD( ... Adjust, Epoch, model, settings);解耦时钟模型[A, omc] DSM_DCM( ... Adjust, Epoch, model, settings);2. 仅使用码或码加多普勒case {Code Doppler, ... Code Only, ... Code (Doppler Smoothing), ... Code (Phase Smoothing)}3. 增加多普勒观测方程if contains(settings.PROC.method, Doppler) [A, omc] DSM_add_Doppler( ... A, omc, Adjust, Epoch, model, settings); end4. 保存矩阵Adjust.A A; Adjust.omc omc;之后最小二乘或卡尔曼滤波直接使用这两个矩阵。十五、构建观测协方差矩阵Adjust createObsCovariance( ... Adjust, Epoch, settings, model.el, model.bore);十六、检查有效观测是否足够n_observations numel(Epoch.exclude);1. 统计被排除观测n_reject sum( ... Epoch.exclude(:) | ... Epoch.cs_found(:) * ... strcmp(settings.IONO.model, GRAPHIC));2.判断剩余观测数量if n_observations - n_reject DEF.MIN_SATS3. 观测不足时退出dx.x(1:3) 0; Adjust.res NaN(numel(Adjust.omc),1); Adjust.float false; Adjust.fixed false; return含义当前历元不给坐标改正残差设为NaN浮点解无效固定解也无效直接结束当前历元十七、历元内迭代卡尔曼滤波case Kalman Filter Iterative1. 首次迭代初始化if it 1 x_pred zeros(size(Adjust.A,2),1); end2. 执行迭代卡尔曼滤波dx KalmanFilterIterative(Adjust, x_pred);3. 累积线性化改正x_pred x_pred - dx.x;4. 判断坐标是否收敛if norm(dx.x(1:3)) DEF.ITERATION_THRESHOLD若坐标改正小于阈值调用Adjust stop_iteration(Adjust, dx); break;5. 未收敛则更新参数Adjust.param Adjust.param dx.x; Adjust.param_pred Adjust.param; Adjust.param_sigma_pred Adjust.param_sigma;十八、普通卡尔曼滤波case Kalman Filter调用[Adjust.param, ... Adjust.param_sigma, ... Adjust.float] ... KalmanFilter( ... Adjust.omc, ... Adjust.A, ... Adjust.param_pred, ... Adjust.param_sigma_pred, ... Adjust.Q);1. 状态更新公式2. 重新计算验后残差[Adjust.res, Adjust.res_doppler] ... calc_res(settings, input, Epoch, model, Adjust, obs);由于卡尔曼滤波更新了状态参数原来的 OMC 是基于预测状态得到的不能直接当作验后残差。所以需要用更新后状态重新建模重新计算理论观测值再计算观测减理论值。3. 普通卡尔曼滤波不进行历元内迭代即每个历元只做一次滤波更新。因此对初值要求更高这也是前面要先执行一次LSQ初始化的原因。十九、单历元最小二乘case No Filter调用dx adjustment(Adjust);1. 判断收敛if norm(dx.x(1:3)) DEF.ITERATION_THRESHOLD坐标改正足够小时Adjust stop_iteration(Adjust, dx); break;2. 未收敛继续迭代Adjust.param Adjust.param dx.x; Adjust.param_pred Adjust.param; Adjust.param_sigma_pred Adjust.param_sigma;更新线性化点后继续计算。二十、判断当前历元是否最终收敛if ~strcmp(filter_type, Kalman Filter) ... norm(dx.x(1:3)) DEF.ITERATION_THRESHOLD普通卡尔曼滤波不进入该检查因为它本来就不进行历元内迭代。对于No FilterKalman Filter Iterative如果达到最大迭代次数后坐标改正仍大于阈值则认为不收敛。不收敛处理Adjust.float false; Adjust.res NaN(numel(Adjust.omc),1);表示当前浮点解无效。注意这里没有将Adjust.param恢复为迭代前状态。因此后续函数是否仍使用这组参数需要检查主程序的无效解处理逻辑。二十一、重置被排除卫星的模糊度if contains(settings.PROC.method, Phase) ... any(Epoch.exclude(:))1. 给所有卫星频率组合编号kk 1:(num_freq*no_sats);2. 找出被排除观测的编号kk kk(Epoch.exclude(:));3. 转换成状态向量中的模糊度下标idx_amb kk NO_PARAM;4. 重置参数和协方差Adjust reset_param_sigma( ... Adjust, idx_amb, settings.ADJ.filter.var_amb);二十二、重置被排除卫星的电离层参数if contains(settings.IONO.model, Estimate) ... any(Epoch.exclude(:,1))只检查第一频率是否被排除1. 获取卫星编号kkk 1:100; kkk kkk(Epoch.exclude(:,1));2. 计算电离层状态位置idx_iono kkk NO_PARAM;如果同时处理相位模糊度位于电离层参数之前if contains(settings.PROC.method, Phase) idx_iono idx_iono num_freq*no_sats; end3. 重置电离层参数Adjust reset_param_sigma( ... Adjust, idx_iono, settings.ADJ.filter.var_iono);使该卫星重新进入时可以重新初始化电离层而不是沿用失锁前的电离层状态。二十三、周跳后重置模糊度if contains(settings.PROC.method, Phase) ... any(Epoch.cs_found(:))只要检测到周跳就将对应模糊度状态重置。1. 观测编号kkkk 1:410; kkkk kkkk(Epoch.cs_found(:));2. 定位模糊度状态idx_cs kkkk NO_PARAM;3. 重置状态Adjust reset_param_sigma( ... Adjust, idx_cs, settings.ADJ.filter.var_amb);二十四、辅助函数一判断接收机钟差是否发生大变化function bool MajorReceiverClockChange(dx, iono_model)普通模型sum_rec_clk ... dx(8) dx(11) dx(14) dx(17) dx(20);解耦时钟模型if strcmp(iono_model, Estimate, decoupled clock) sum_rec_clk ... dx(8) dx(9) dx(10) dx(11) dx(12); end二十五、辅助函数二停止迭代并保存结果function Adjust stop_iteration(Adjust, dx)1. 标记浮点解有效Adjust.float true;2. 保存估计参数Adjust.param Adjust.param dx.x;3. 保存验后残差Adjust.res dx.v;4. 保存参数协方差Adjust.param_sigma dx.Qxx;二十六、辅助函数三重置参数和协方差function Adjust reset_param_sigma( ... Adjust, idx, initial_var)1. 参数置零Adjust.param(idx) 0;2. 清除与其他参数的相关性Adjust.param_sigma(idx,:) 0; Adjust.param_sigma(:,idx) 0;3. 重设对角方差sz size(Adjust.param_sigma); idx_ sub2ind(sz, idx, idx); Adjust.param_sigma(idx_) initial_var;二十七、辅助函数四卡尔曼滤波后重新计算残差function [res, res_doppler] ... calc_res(settings, input, Epoch, model, Adjust, obs)1. 使用滤波后的参数重新建模Adjust.param_pred Adjust.param;2. 重新计算几何距离los vecnorm2( ... model.Rot_X - Adjust.param(1:3)); model.rho repmat( ... los, 1, settings.INPUT.proc_freqs);3. 重新计算接收机钟差model getReceiverClockBiases( ... model, Epoch, Adjust.param, settings);4. 重新计算误差项和理论观测[model, Epoch] modelErrorSources(...); [code_model, phase_model, doppler_model] ... model_observations(...);二十八、码相位残差的排列方式s_f numel(Epoch.sats) * ... settings.INPUT.proc_freqs;1. 初始化交替排列的残差向量res zeros(s_f, 1); code_row 1:2:2*s_f; phase_row 2:2:2*s_f;2. 码残差res(code_row,:) ... (Epoch.code(:) - code_model(:)) .* ~exclude;3. 相位残差usePhase ~Epoch.cs_found(:); res(phase_row,:) ... (Epoch.phase(:) - phase_model(:)) .* ... ~exclude .* usePhase;4. GRAPHIC模式if strcmp(settings.IONO.model, GRAPHIC) res(code_row,:) []; end二十九、多普勒残差if contains(settings.PROC.method, Doppler)计算res_doppler ... (Epoch.doppler(:) doppler_model(:)) .* ~exclude;
)
