0.前言

平滑技术作为事后或准实时数据处理的一种方法，可以在一定程度上提高数据处理的精度，在测绘领域获得了广泛的应用。平滑技术总的来说分为三类：固定区间平滑(Fixed—Interval Smoot—hing)、固定点平滑(Fixed—Point SmoOthing) 和 固定滞后平滑(Fixed—Lag Smoothing)。其中在数据后处理中应用最为广泛的方法就是固定区间平滑，其原理图如下图所示。它是在前向Kalman滤波的基础上进行的反向滤波处理，充分利用区间内所有时刻的测量值对某一时刻的状态进行估计。该方法能提供比单向滤波更高的精度，同时在卫星信号失锁段也是一种很好的桥接算法。
在这里插入图片描述

1. RTS算法

R-T-S算法是一种固定区间最优平滑算法。许多文献中将R-T-S固定区间最优平滑算法应用到GPS／INS组合导航系统中，结果R-T-S固定区间平滑算法的精度比Kalman滤波的精度高，且该方法计算简单，易于实现，是一种有效的事后处理方法。由于该方法的优势主要在于GPS失锁时刻，而在不丢星时并无显著改进。
在这里插入图片描述

R—T—S平滑算法由前向滤波和后向滤波组成，前向滤波是经典的Kalman滤波器，用来估计每一时刻的状态，后向滤波是在前向滤波的基础上重复利用部分数据，以获取更精确的状态估计值。
RTS平滑可以用于计算以下闭合解
在这里插入图片描述
即利用观测$y_{1:T}$ 估计 $k$ 时刻的系统状态后验分布，该过程分成前向和后向两步：

  • 前向递推（Forward Recursion）：
    在这里插入图片描述
  • 后向递推（Backward Recursion）：
    在这里插入图片描述
    从初始时刻$1$到 $T$时刻完成 $T$ 次前向递推后，再由 $T$时刻经过 $T$次后向递推，完成RTS平滑过程。其中前向递推过程即为卡尔曼滤波过程，而前向递推获得的最后 $T$时刻的状态估计${m}_{T}$和协方差矩阵${P}_{T}$即为后向递推过程的初始状态估计${m}_{T}^s$与协方差矩阵${P}_{T}^s$，即${m}_{T}={m}_{T}^s$，${P}_{T}={P}_{T}^s$ 。

2. 代码实现

def rts_smoother(pos_meas, vel_meas, pos_true, t, samples_count, t_step, q_proc_noise_cov, r_meas_noise_cov,
                 flg_debug_prop_only, flg_debug_fwd_only, update_interval_indices, a_trans_mat, c_obs_mat):
    # RTS smoother initialization
    pos_est_ini = pos_meas[0]
    pos_var_ini = q_proc_noise_cov
    pos_est_pred_fwd = np.zeros(samples_count)
    pos_est_corr_fwd = np.zeros(samples_count)
    pos_est_corr = np.zeros(samples_count)
    pos_var_pred_fwd = np.zeros(samples_count)
    pos_var_corr_fwd = np.zeros(samples_count)
    pos_var_corr = np.zeros(samples_count)

    # RTS smoother forward pass
    for i in range(0, samples_count):
        if i == 0:
            pos_est_pred_fwd[0] = pos_est_ini
            pos_var_pred_fwd[0] = pos_var_ini
        else:
            pos_var_pred_fwd[i] = a_trans_mat * pos_var_corr_fwd[i - 1] * a_trans_mat + q_proc_noise_cov
            pos_est_pred_fwd[i] = a_trans_mat * pos_est_corr_fwd[i - 1] + t_step * vel_meas[i]
        if flg_debug_prop_only == 1:
            kalman_gain = 0
        elif (i % update_interval_indices) == 0:
            kalman_gain = pos_var_pred_fwd[i] * c_obs_mat / (
                    c_obs_mat * pos_var_pred_fwd[i] * c_obs_mat + r_meas_noise_cov)
        else:
            kalman_gain = 0
        pos_var_corr_fwd[i] = (1 - kalman_gain * c_obs_mat) * pos_var_pred_fwd[i]
        pos_est_corr_fwd[i] = pos_est_pred_fwd[i] + kalman_gain * (pos_meas[i] - c_obs_mat * pos_est_pred_fwd[i])

    # RTS smoother backward pass
    if flg_debug_fwd_only == 1:
        pos_var_corr = pos_var_corr_fwd
        pos_est_corr = pos_est_corr_fwd
    else:
        pos_est_corr[-1] = pos_est_corr_fwd[-1]
        pos_var_corr[-1] = pos_var_corr_fwd[-1]
        for i in range(samples_count - 1, 0, -1):
            pos_est_corr[i - 1] = \
                pos_est_corr_fwd[i - 1] + (pos_var_corr_fwd[i - 1] * a_trans_mat / pos_var_pred_fwd[i]) * \
                (pos_est_corr[i] - pos_est_pred_fwd[i])
            pos_var_corr[i - 1] = \
                pos_var_corr_fwd[i - 1] + (pos_var_corr_fwd[i - 1] * a_trans_mat / pos_var_pred_fwd[i]) * \
                (pos_var_corr[i] - pos_var_pred_fwd[i]) * (pos_var_corr_fwd[i - 1] * a_trans_mat / pos_var_pred_fwd[i])

    # Post process results
    pos_est_err = pos_est_corr - pos_true
    pos_est_err_mean = np.mean(pos_est_err)
    pos_est_err_mod_avg = np.mean(np.abs(pos_est_err))
    pos_est_rmse = np.sqrt(np.sum(pos_est_err ** 2) / samples_count)
    pos_est_err_std = np.std(pos_est_err)
    pos_3sigma = 3 * np.sqrt(pos_var_corr)
    print('*** POST-PROCESS ****\n'
          f'Process model variance = {q_proc_noise_cov:1.5f}\n'
          f'Measurement model variance = {r_meas_noise_cov:1.5f}\n'
          f'Average error magnitude = {pos_est_err_mod_avg:1.3f} m\n'
          f'Root mean square error = {pos_est_rmse:1.3f} m\n'
          f'3 sigma error = {3 * pos_est_err_std:1.3f} m')

3. 参考链接

https://blog.csdn.net/weixin_42647783/article/details/106035691
https://github.com/sandropapais/batch_rts_smoother/blob/main/lg_batch_rts_smoother.py