后向平滑与抗差估计融合的SRCKF滤波

孙 鹏，赵长胜，谭兴龙，张立凯

(江苏师范大学地理测绘与城乡规划学院，江苏 徐州 221116)

后向平滑与抗差估计融合的SRCKF滤波

孙 鹏，赵长胜，谭兴龙，张立凯

(江苏师范大学地理测绘与城乡规划学院，江苏 徐州 221116)

将后向平滑平方根容积卡尔曼滤波用于GPS动态单点定位数据处理，并探讨了粗差对后向平滑滤波的影响。借鉴经典卡尔曼滤波抗差估计思想，给出平方根容积卡尔曼滤波的抗差算法以抵抗量测粗差，而当判断不含粗差时使用后向平滑算法，在有效提高滤波精度的同时避免了抗差滤波对每个历元都需进行迭代运算。实测GPS动态数据验证了算法的有效性。

非线性滤波；平方根容积卡尔曼滤波；抗差滤波；后向平滑

扩展卡尔曼滤波(EKF)、无迹卡尔曼滤波(UKF)及粒子滤波(PF)是常用的非线性滤波算法[1]。EKF存在线性化误差，且常难以获得雅可比矩阵求导[2]，但在单点定位中，观测函数非强非线性函数，故常采用EKF进行处理[3-4]。UKF无需线性化，处理强非线性系统时精度明显优于EKF[5]，但不适当的采样策略和参数选取可能导致UKF滤波发散[6]。PF则存在计算量大、粒子退化等问题[7-8]。容积卡尔曼滤波(CKF)[9-10]采用基于三阶球面-径向变换的容积准则计算高斯加权积分近似值，数学推导过程严格，在维数较高时也可取得良好的滤波效果[11]。本文将一种后向平滑平方根容积卡尔曼滤波(BS-SRCKF)用于单点定位，给出SRCKF的抗差算法，并探讨了抗差算法与后向平滑的融合。

1 平方根容积卡尔曼滤波(SRCKF)

计算机舍入误差可能导致协方差阵失去对称正定性，导致滤波失败，而平方根容积卡尔曼滤波只计算并传递协方差阵平方根因子，在提高数值稳定性的同时减小了计算量[12]。当系统状态为n维时，三阶CKF取2n个容积点用于积分近似值的计算。设有离散非线性系统

(1)

(2)

1.1 时间更新

求解、更新容积点

(3)

状态预测值及其协方差阵平方根因子

(4)

tria()表示一种三角化运算[9]，其中

1.2 量测更新

求并更新容积点

(5)

求预测观测值、新息协方差阵的平方根因子与互协方差阵

(6)

式中

计算增益矩阵，更新状态及其协方差阵平方根因子

(7)

2 后向平滑平方根容积卡尔曼滤波(BS-SRCKF)

后向平滑滤波可有效提升滤波精度，在平方根容积卡尔曼滤波下，其流程可表示为

(1) 用式(3)—式(7)进行前向滤波得k时刻滤波值。

(2) 按下式获取k-1时刻状态平滑值[13]

(8)

(3) 将后向平滑值作为k时刻初值再次按式(3)—式(7)前向滤波。

3 抗差平方根容积卡尔曼滤波(R-SRCKF)

当预测信息准确时，粗差将直接反映在预测残差中。通过预测残差及其协方差阵可构建抗差因子，若存在粗差，放大该观测值的噪声协方差以降低该观测值在状态估值计算中的权重，即可抵抗粗差影响。

k时刻预测残差及其协方差阵为

(9)

(10)

则参考Huber法可求得抗差因子[14]

(11)

式中，c为阈值，可取为c=1.0～1.5，当标准化残差大于c时，认为该观测值含粗差。c的取值越小，对粗差的判断越为严格。求得抗差因子后，膨胀量测噪声协方差

(12)

在非对角线上，按双因子等价协方差模型取

(13)

以新的量测噪声协方差阵按式(6)、式(7)求得抗差滤波值及其协方差阵，并对量测更新过程进行迭代直至收敛。

4 带抗差估计的后向平滑平方根容积卡尔曼滤波(BS-R-SRCKF)

后向平滑滤波采用k时刻滤波结果对k-1时刻滤波结果进行平滑。由式(8)可知，若k时刻存在量测粗差，则k时刻滤波值将不准确，进而后向平滑值，亦即第二次前向滤波的初值也将受到粗差影响，可见，当粗差存在时，BS-SRCKF相比SRCKF受粗差影响更大。同时，单纯使用抗差滤波处理粗差需要进行迭代求解，这将增加计算负担。本文在平方根容积滤波下将后向平滑算法与抗差滤波融合(如图1所示)，在判定观测值含有粗差时迭代求解抗差滤波值，否则使用后向平滑算法提高滤波精度。

图1 后向平滑与抗差估计融合的SRCKF流程

5 算 例

取一航摄飞机的一段GPS观测数据进行滤波解算，采用双频消电离层组合伪距，并改正对流层延迟误差。状态参数取

伪距量测噪声方差初值取1 m2。系统噪声协方差阵Q取

Q=diag([0.09 0.09 0.09 9 0.09 0.09 0.09 9])

P0=diag([5005005001000100100100100])

分别使用SRCKF和BS-SRCKF进行滤波，以CSRS精密单点定位在线处理输出值为参考，在WGS-84坐标系下两种滤波误差如图2所示。

现每隔50历元在其中一颗卫星的组合伪距上加50 m粗差，分别使用BS- SRCKF、BS-R-SRCKF进行滤波，所得结果如图3所示。

表1为各方案误差的均方根误差。

表1 各方案均方根误差 m

滤波结果表明，后向平滑算法可以有效提高滤波精度，但当量测值含有粗差时，粗差对滤波的影响明显，而融合抗差估计的后向平滑可有效抵抗粗差、提高滤波的精度。

---表示SRCKF，…表示 BS-SRCKF图2 SRCKF与BS-SRCKF滤波结果

…为BS- SRCKF，—为BS-R-SRCKF图3 BS-SRCKF与BS-R-SRCKF滤波结果

6 结 语

本文将后向平滑平方根容积卡尔曼滤波用于动态单点定位数据处理，针对粗差对后向平滑滤波的影响，给出了平方根容积卡尔曼滤波下的抗差因子取值。将后向平滑与抗差估计融合，当观测值含有粗差时进行抗差迭代，当不含粗差时使用后向平滑滤波，提高滤波精度的同时避免抗差滤波每个历元都迭代带来的计算量增大问题。实测GPS动态数据的计算结果表明，后向平滑可有效提高滤波精度，与抗差估计的融合也切实可行。

[1] 赵长胜.测量数据处理研究[M].北京：测绘出版社，2013.

[2] 谭兴龙,王坚,赵长胜. 神经网络辅助的GPS/INS组合导航自适应UKF算法[J]. 测绘学报,2015,44(4):384-391.

[3] 祁芳. 卡尔曼滤波算法在GPS非差相位精密单点定位中的应用研究[D].武汉：武汉大学,2003.

[4] 王坚,王金岭,高井祥. 基于抗差EKF的GNSS导航模型研究[J]. 中国矿业大学学报,2008,37(4):473-477.

[5] 吴江飞,雷辉. 星载GPS卫星定轨的UKF-EKF算法[J]. 测绘学报，2014,43(5):446-451.

[6] 王小旭,潘泉,黄鹤,等. 非线性系统确定采样型滤波算法综述[J]. 控制与决策，2012,27(6):801-812.

[7] 聂建亮,杨元喜,吴富梅. 一种基于改进粒子滤波的动态精密单点定位算法[J]. 测绘学报,2010,39(4):338-343.

[8] 薛长虎,聂桂根,汪晶. 扩展卡尔曼滤波与粒子滤波性能对比[J]. 测绘通报,2016(4):10-14.

[9] ARASARATNAM I, HAYKIN S. Cubature Kalman Filters[J].IEEE Transactions on Automatic Control,2009,54(6):1254-1269.

[10] ARASARATNAM I. Cubature Kalman Filtering: Theory & Applications[D].Hamilton：McMaster University, 2009.

[11] 唐李军. Cubature卡尔曼滤波及其在导航中的应用研究[D].哈尔滨：哈尔滨工程大学,2012.

[12] 郝燕玲,杨峻巍,陈亮,等 平方根容积卡尔曼滤波器[J]. 弹箭与制导学报,2012,32(2):169-172.

[13] 汤霞清,黄湘远,武萌,等. 平方根形式的CKF/后向平滑非线性滤波研究[J]. 装甲兵工程学院学报,2015(2):65-69.

[14] 杨元喜.自适应动态导航定位[M].北京：测绘出版社，2006.

[15] 高为广. 自适应融合导航理论与方法及其在GPS和INS中的应用[D].郑州：信息工程大学,2005.

Square-root Cubature Kalman Filter with Backward-smoothing and Robust Estimation

SUN Peng，ZHAO Changsheng，TAN Xinglong， ZHANG Likai

(School of Geodesy and Geomatics, Jiangsu Normal University, Xuzhou 221116， China)

Backward-smoothing square-root cubature Kalman filter(BS-SRCKF) is used to caculate observation data of GPS dynamic single point positioning,and the gross errors’ bad influence on backward-smoothing filter is analyzed in this article. In order to reduce gross errors’ impacts, robust SRCKF is put forward referring to robust Kalman filter. And when coarse errors don’t exist,backward-smoothing filter can be used to improve the precision of the results and avoid iterative computation if using robust filter each epoch.Measured data is adopted to prove that the algorithm is effective.

nonlinear filtering; SRCKF; robust filter; backward smoothing

孙鹏，赵长胜，谭兴龙，等.后向平滑与抗差估计融合的SRCKF滤波[J].测绘通报，2017(4)：17-20.

10.13474/j.cnki.11-2246.2017.0111.

2016-07-22；

2016-12-31

江苏省自然科学青年基金(BK20150236)；江苏师范大学研究生科研创新计划重点项目(2016YZD021)

孙 鹏(1991—)，男，硕士生，研究方向为GNSS数据处理。E-mail：spcxs@sohu.com

P207

A

0494-0911(2017)04-0017-04