raPPPid之calc_float_solution.m

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.bool

1. 构造临时最小二乘设置

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 cm

norm(dx.x(1:3)) > 0.05

如果本次迭代坐标变化超过0.05 m，就需要重新计算：几何距离；卫星高度角和方位角；对流层映射函数；潮汐改正；天线相位中心；地球自转改正；相关方向性误差。

2. 接收机钟差发生明显变化

MajorReceiverClockChange(...)

3. 调用核心误差建模函数（modelErrorSources）

if 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_sats+1: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); end

4. 保存矩阵

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_SATS

3. 观测不足时退出

dx.x(1:3) = 0; Adjust.res = NaN(numel(Adjust.omc),1); Adjust.float = false; Adjust.fixed = false; return

含义：

当前历元不给坐标改正；

残差设为NaN；

浮点解无效；

固定解也无效；

直接结束当前历元

十七、历元内迭代卡尔曼滤波

case 'Kalman Filter Iterative'

1. 首次迭代初始化

if it == 1 x_pred = zeros(size(Adjust.A,2),1); end

2. 执行迭代卡尔曼滤波

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 Filter；
• Kalman 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; end

3. 重置电离层参数

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;