给Cssrlib加注释（2）--有关滤波前的初始化-CSDN博客

本文链接：https://blog.csdn.net/m0_59951791/article/details/136433474

补充与修正

时至今日发现作者好像对代码进行了更新，尤其是在选择星历方面：

def findeph(nav, t, sat, iode=-1, mode=0):
    """ find ephemeris for sat """
    sys, _ = sat2prn(sat)
    eph = None
    tmax = MAXDTOE_t[sys]
    tmin = tmax + 1.0
    for eph_ in nav:
        if eph_.sat != sat or (iode >= 0 and iode != eph_.iode):
            continue
        if eph_.mode != mode:
            continue
        dt = abs(timediff(t, eph_.toe))
        if dt > tmax or eph_.mode != mode:
            continue
        if iode >= 0:
            return eph_
        if dt <= tmin:
            eph = eph_
            tmin = dt

    return eph

采用了与GAMP相同的二分法查找，并确保iode相同

udstate()函数--滤波前的初始化

在依次给卫星打上系统标签后进行初始化，首先初始化状态转移矩阵：

nx = self.nav.nx
Phi = np.eye(nx)

这里的nx表示状态维度数量，计算原则可以在前面类的定义中找到：

self.nav.na = (3 if self.nav.pmode == 0 else 6)

self.nav.na += self.nav.ntrop + self.nav.niono

# State vector dimensions (including slant iono delay and ambiguities)
 #self.nav.nx = self.nav.na+uGNSS.MAXSAT*self.nav.nf

首先根据定位模式判断估计数量，静态估计三维坐标，紧接着加上对流层和电离层延迟参数，nf表示频率得数量，这里写死为双频，则模糊度数量为卫星数量*频率数量(MAXSAT * self.nav.nf)

下一步初始化P阵

self.nav.P[0:nx, 0:nx] = Phi@self.nav.P[0:nx, 0:nx]@Phi.T

这里应该是卡尔曼滤波中更新状态向量的方差--协方差阵，根据参考文献[3]式(2.44)，这段代码应该是设置的前半段，紧接着代码设置了过程噪声(Q)

$\hat{P}_k^{-}=\Phi_{k, k-1} \hat{P}_{k-1} \Phi_{k, k-1}^T+Q_{k-1}$

接下来设置过程噪声：

dP = np.diag(self.nav.P)
dP.flags['WRITEABLE'] = True
dP[0:self.nav.nq] += self.nav.q[0:self.nav.nq]*tt

我们来逐行分析：

第一行：提取方差-协方差矩阵的对角线元素，这些元素代表的是状态变量的方差

第二行：确保dP数组是可编辑的，这样能修改它的值

第三行：self.nav.q在类中声明的是过程噪声，这里成时间t是不是说明其随时间是发散的（随机游走）

上文中，对于self.nav.q有如下定义：

        # Process noise
        #
        self.nav.q = np.zeros(self.nav.nq)
        self.nav.q[0:3] = self.nav.sig_qp**2

        # Velocity
        if self.nav.pmode >= 1:  # kinematic
            self.nav.q[3:6] = self.nav.sig_qv**2

        if self.nav.trop_opt == 1:  # trop is estimated
            # Tropo delay
            if self.nav.pmode >= 1:  # kinematic
                self.nav.q[6] = self.nav.sig_qztd**2
            else:
                self.nav.q[3] = self.nav.sig_qztd**2

        if self.nav.iono_opt == 1:  # iono is estimated
            # Iono delay
            if self.nav.pmode >= 1:  # kinematic
                self.nav.q[7:7+uGNSS.MAXSAT] = self.nav.sig_qion**2
            else:
                self.nav.q[4:4+uGNSS.MAXSAT] = self.nav.sig_qion**2

同样，我们逐行分析上述代码，过程噪声的设置对于滤波结果非常重要：

首先第一行初始化大小，这里nq在静态定位模式下等于3，紧接着出现了self.nav.sig_qp，这是过程噪声的标准差，在类中如下定义，这里应该就是先验信息：

        # Process noise sigma
        #
        if self.nav.pmode == 0:
            self.nav.sig_qp = 100.0/np.sqrt(1)     # [m/sqrt(s)]
            self.nav.sig_qv = None
        else:
            self.nav.sig_qp = 0.01/np.sqrt(1)      # [m/sqrt(s)]
            self.nav.sig_qv = 1.0/np.sqrt(1)       # [m/s/sqrt(s)]
        self.nav.sig_qztd = 0.1/np.sqrt(3600)      # [m/sqrt(s)]
        self.nav.sig_qion = 10.0/np.sqrt(1)        # [m/s/sqrt(s)]

从中可以看出，当静态模式时(self.nav.pmode == 0)，位置标准差设置为100米每根号秒并且不设置速度标准差，而在动态定位模式下，位置标准差设置为0.01米每根号秒，我的理解是动态模式下认为动起来是更正常的。速度标准差设置为1，对流层标准差设置为0.1米每小时，说明其变化缓慢，而电离层设置为10米每秒说明其变化更剧烈。

上上代码第二行就是把标准差变成方差，下述代码也是判断不同模式下应该加入哪些过程噪声。接下来按照频率顺序更新卡尔曼滤波中的状态元素：

在更新前首先设置矩阵大小和初始化，这里有一处标记：

                self.nav.outc[i, f] += 1
                reset = (self.nav.outc[i, f] >
                         self.nav.maxout or np.any(self.nav.edt[i, :] > 0))
                if sys_i not in obs.sig.keys():
                    continue

这里的self.nav.outc可能表示卫星i在第f个频率上有连续多少个时间点没被观测到或未处理，reset表示是否需要重置，判断条件是未观测次数大于阈值maxout或者edt矩阵非0，这里的edt矩阵是在quedt()函数里更新的，具体内容可见上一个板块。

接下来更新模糊度：

                j = self.IB(sat_, f, self.nav.na)
                if reset and self.nav.x[j] != 0.0:
                    self.initx(0.0, 0.0, j)
                    self.nav.outc[i, f] = 0

                    if self.nav.monlevel > 0:
                        self.nav.fout.write(
                            "{}  {} - reset ambiguity  {}\n"
                            .format(time2str(obs.t), sat2id(sat_),
                                    obs.sig[sys_i][uTYP.L][f]))

第一行中IB是获取在矩阵中的位置：

    def IB(self, s, f, na=3):
        """ return index of phase ambguity """
        idx = na+uGNSS.MAXSAT*f+s-1
        return idx

这里na为3(三维坐标)，uGNSS.MAXSAT*f表示已经处理过的卫星数，因为是按照频率顺序，一次循环就能处理一个频率，现在是第一频率，所以f是0，s-1表示卫星s的位置，这里s-1是因为卫星编号从1开始，但是索引从0开始。

下一行判断是否要重置（reset为真且状态向量中模糊度参数不为0），如果需要，进入initx()函数进行重置:

    def initx(self, x0, v0, i):
        """ initialize x and P for index i """
        self.nav.x[i] = x0
        for j in range(self.nav.nx):
            self.nav.P[j, i] = self.nav.P[i, j] = v0 if i == j else 0

即如果在对角线上，设置为v0，如果不是，设置为0，紧接着将计数器清空，接下来输出消息。

接下来初始化电离层延迟：

                if self.nav.niono > 0:
                    # Reset slant ionospheric delay estimate
                    #
                    j = self.II(sat_, self.nav.na)
                    if reset and self.nav.x[j] != 0.0:
                        self.initx(0.0, 0.0, j)

                        if self.nav.monlevel > 0:
                            self.nav.fout.write("{}  {} - reset ionosphere\n"
                                                .format(time2str(obs.t),
                                                        sat2id(sat_)))

相同的思路，首先返回参数所在的下标，判断是否需要初始化，并输出消息。

下面获取双拼的频率和伪距：

                    sig1 = obs.sig[sys[i]][uTYP.C][0]
                    sig2 = obs.sig[sys[i]][uTYP.C][1]

                    pr1 = obs.P[i, 0]
                    pr2 = obs.P[i, 1]

                    # Skip zero observations
                    #
                    if pr1 == 0.0 or pr2 == 0.0:
                        continue

                    if sys[i] == uGNSS.GLO:
                        if sat[i] not in self.nav.glo_ch:
                            print("glonass channed not found: {:d}"
                                  .format(sat[i]))
                            continue
                        f1 = sig1.frequency(self.nav.glo_ch[sat[i]])
                        f2 = sig2.frequency(self.nav.glo_ch[sat[i]])
                    else:
                        f1 = sig1.frequency()
                        f2 = sig2.frequency()

接下来计算第一个频率上的电离层延迟，这里我简单推导了下，从简化的伪距观测方程开始：

$P_1=\rho +dt+\delta t+I_1+T$

$P_2=\rho +dt+\delta t+I_2+T$

$P_1-P_2=I_1-I_2\ \ f_{2}^{2}I_2=f_{1}^{2}I_1$

$P_1-P_2=\left( 1-f_{1}^{2}/f_{2}^{2} \right) I_1$

$I_1=\left( P_1-P_2 \right) /\left( 1-f_{1}^{2}/f_{2}^{2} \right)$

因此代码里这样写：

ion[i] = (pr1-pr2)/(1.0-(f1/f2)**2)

接下来求解模糊度：

 bias[i] = cp - pr/lam + 2.0*ion[i]/lam*(f1/fi)**2

至于表达式为什么长这样，我将在下一节详细介绍有关模糊度的东西

接下来把存下来的模糊度写入类中

                if bias[i] != 0.0 and self.nav.x[j] == 0.0:

                    self.initx(bias[i], self.nav.sig_n0**2, j)

                    if self.nav.monlevel > 0:
                        sig = obs.sig[sys_i][uTYP.L][f]
                        self.nav.fout.write(
                            "{}  {} - init  ambiguity  {} {:12.3f}\n"
                            .format(time2str(obs.t), sat2id(sat[i]),
                                    sig, bias[i]))

                if self.nav.niono > 0:
                    j = self.II(sat[i], self.nav.na)
                    if ion[i] != 0 and self.nav.x[j] == 0.0:

                        self.initx(ion[i], self.nav.sig_ion0**2, j)

                        if self.nav.monlevel > 0:
                            self.nav.fout.write(
                                "{}  {} - init  ionosphere      {:12.3f}\n"
                                .format(time2str(obs.t), sat2id(sat[i]),
                                        ion[i]))

在采用IB()函数获取模糊度参数在状态转移矩阵的位置后，判断模糊度是否初始化，调用initx 函数来初始化模糊度，其中 bias[i] 是模糊度的估计值，self.nav.sig_n0**2 是模糊度的初始方差，j 是模糊度在状态向量中的位置。再次调用initx函数：

    def initx(self, x0, v0, i):
        """ initialize x and P for index i """
        self.nav.x[i] = x0
        for j in range(self.nav.nx):
            self.nav.P[j, i] = self.nav.P[i, j] = v0 if i == j else 0

首先将状态向量x的第j个位置初始化为bias，然后遍历方差-协方差矩阵，初始化P阵，总的来说，就是先把模糊度的值赋值到状态向量里，再把模糊度的方差赋值到P阵中。