改进的非线性鲁棒EKF算法及其应用
合集下载
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
[4-5]
产生， 低轨卫星采用 J2 摄动模型。在仿真产生观测数据时， GPS 伪距和伪距率误差分别为 50 m (1σ ) 和 0.2 m/s (1σ ) 。下面 分别应用文献[6]提出的算法和本文的算法进行定轨计算。初 始状态变量的确定应用迭代最小二乘解算， P 的初值取为
其中 ξ k 中包含原始白噪声 w k 和 ϕ( x k k ) 在 x̂ k 处展开时的所 （1） （2）
η k 包括原有噪声 v k 和泰勒展开时 有高于二阶的高阶项 R(o) ； ξ k 和 η k 带有未知时变统计： 的所有高于二阶的高阶项 R(o) ； Eξ k = q k ， Cov[ξ k ξ j] = Q k δ kj
i i (z s - z u)( ż s - ż u)]/Ri} + Dρ̇ i , i=1， 2， …， n i s i s i s i s i s i s i s i s i s i s
（21）
（22） （23）
φ
)
r̂ k =Leabharlann (1 - d k - 1)r̂ k - 1 + d k - 1(z k - h( x̂ k/k - 1 k )) ̂ = (1 - d ) R ̂ R k k-1 k - 1 + d k - 1(ε ε - H k P k/k - 1 H )
w k、 测向量， h 为 m 维可微向量函数； m 维的高斯 v k 分别为 n、
白噪声， 它们彼此独立， 均值和协方差分别为：
Ew k = q Ev k = r Cov[w k w j] = Qδ kj Cov[v k v j] = Rδ kj
δ kj 是 Kronecker 函数。 其中， q、 Q、 r、 R 均未知时， 若采用经典 EKF 滤波， 将会导致滤
时变噪声统计估计器：
q̂ k = (1 - d k - 1)q̂ k - 1 + d k - 1( x̂ k - ϕ( x̂ k - 1 k - 1)) ̂ = (1 - d )Q ̂ Q k k-1 k-1 + d k - 1(k ε ε k + P k - φ k - 1 P
T k k k T k T k k T k-1 k-1
波误差变大， 甚至滤波发散。文献 [6] 提出了非线性鲁棒 Kalman 滤波， 其原理如下： 将式 （1） 中的 ϕ 围绕 k 时刻的状态估值 x̂ k 展开成泰勒级， 同时将观测方程中 h 在一步预测值 x̂ k/k - 1 处展开数， 进而得到：
xk + 1 = zk = ¶ϕ ¶ϕ x + (ϕ( x̂ k k ) x̂ ) + ξ k ¶x̂ k k ¶x̂ k k
Computer Engineering and Applications 计算机工程与应用
2011， 47 （3）
207
改进的非线性鲁棒 EKF 算法及其应用
2 吴志华 1， ， 丁杨斌 1， 申功勋 1 2 WU Zhihua1， ， DING Yangbin1， SHEN Gongxun1
1.北京航空航天大学 宇航学院， 北京 100083 2.中北大学 电子与计算机科学技术学院， 太原 030051 1.School of Aeronautics， Beijing University of Aeronautics and Astronautics， Beijing 100083， China 2.Institute of Electronic and Computer Science and Technology， North University of China， Taiyuan 030051， China WU Zhihua， DING Yangbin， SHEN Gongxun.Modified nonlinear robust EKF algorithm and its puter Engineering and Applications， 2011， 47 （3） ： 207-209. Abstract： Because of the uncertain noise statistics in nonlinear system， the filter error of the classical EKF （Extended Kalman Filter） will be enlarged even lead to filter divergence.To overcome this limitation， a novel method is proposed， it uses the method of fictitious noise compensating， and time-varying noise statistics can be estimated and compensated on line， so it improves the robust of nonlinear filter.At last， the proposed method is used in space-borne GPS orbit determination， the result shows that the novel method has better stability of filter and accuracy of orbit determination. Key words：nonlinear system； time-varying noise； robust EKF； space-born GPS orbit determination 摘 要： 非线性系统由于噪声统计的不确定性， 经常使得常规扩展 Kalman 滤波器滤波误差放大， 甚至引起滤波发散， 为此提出了
观测方程为： ρù Zk = é ê ú = h( X k k ) + v k ëρ̇ û 其中伪距、 伪距率分别表示为[8]：
ρi = Ri + C ´ b + Dρi （i=1， 2， 3， …， n） ρ̇ ={[( x - x u)( ẋ - ẋ u) + ( y - y u)( ẏ - ẏ u) +
（3） （4）
1
非线性鲁棒 Kalman 滤波
对于离散非线性随机系统：
x k + 1 = ϕ( x k k ) + w k z k = h( x k k ) + v k
¶h x + (h( x̂ k ) - ¶h x̂ k/k - 1) + η k k/k - 1 ¶x̂ k/k - 1 k ¶x̂ k/k - 1
T k T k
3 算法在星载 GPS 定轨中的应用 3.1 星载 GPS 定轨方程
在星载 GPS 低轨卫星定轨中， 状态和观测方程均为非线 性的， 可表示为： ìẊ = f ( X ) + w k í îZ k = h( X k k ) + v k 状态向量 X 取为：
X = [ x y z ẋ ẏ ż b]
2
提出的自适应非线性鲁棒扩展 Kalman 滤波算法
在上面的对状态和观测统计特性的方差估计中， 为了
3.2
算例分析
文中低轨卫星和 24 颗 GPS 卫星星历数据均由 STK 软件
达到无偏性， 在式 （12） 和 （14） 中引入了减法运算， 使得噪声 ̂ ̂ 统计二阶矩的估计 Q 和 R 分别失去半正定性和正定性， 从 而引起滤波发散
208
2011， 47 （3）
Eη k = r k ， Cov[η k η j] = R k δ kj
Computer Engineering and Applications 计算机工程与应用
虚拟噪声 ξ k 和 η k 分别补偿了线性化状态模型和线性化 观测模型的误差， 将非线性系统的滤波问题转化为线性滤 波问题。对式 （3） 和 （4） 运用传统 Kalman 滤波方法， 就得到带 未知噪声统计的非线性系统 （1） 、 （2） 的鲁棒扩展 Kalman 滤波 方程[6]：
k
Dρi 星到用户卫星间的几何距离； C 为光速； b 为接收机钟差； Dρ̇ i 为伪距率误差。 为伪距误差；
0 < b < 1 称为遗忘因子， 其取值一般在 0.950～0.995。对于慢
时变噪声统计， 应取较大的 b。该算法中对统计特性的估计为 无偏估计。
对于式 （19） 中连续的状态方程进行离散化后得到如式 （1） 的表示形式， 其与式 （19） 的观测方程一起组成如式 （1） 和 （2） 的离散非线性随机系统。
（14）
式中 ( x y z ) 和 ( ẋ ẏ ż ) 为第 i 颗 GPS 卫星的位置和速度，
i Ri = ( xis - x u)2 + ( yis - y u)2 + ( z s - z u)2 为观测到的第 i 颗 GPS 卫
其中： φk =
¶ϕ( x k ) | | | ，H k = ¶h | , d = (1 - b)/(1 - bk + 1) ， ¶x |x = x̂ ¶x |x = x̂ k/k - 1 k
ϕ 为 n 维可微向量函数； z k 为 m 维观 其中 x k 为 n 维状态向量，
基金项目： 国家部委创新项目 （No.7130617） 。 作者简介： 吴志华 （1972—） ， 女， 博士后， 副教授， 研究方向： 导航、 制导与控制。 收稿日期： 2009-09-04 修回日期： 2009-11-05
x̂ k = x̂ k/k - 1 + k k ε k x̂ k/k - 1 = ϕ( x̂ k - 1 k - 1) + q̂ k - 1 ε k = z k - h( x̂ k/k - 1 k ) - r̂ k - 1 ̂ )- 1 k k = P k/k - 1 H ( H k P k/k - 1 H + R k-1
一种采用虚拟噪声的补偿方法， 对时变的噪声统计进行在线估计且加以补偿， 提高了非线性滤波的鲁棒性。以星载 GPS 定轨为 研究对象， 应用该方法进行滤波定轨， 结果表明， 方法具有滤波稳定性和定轨结果的精确性。 关键词： 非线性系统； 时变噪声； 鲁棒 EKF； 星载 GPS 定轨 DOI： 10.3778/j.issn.1002-8331.2011.03.061 文章编号： 1002-8331 （2011） 03-0207-03 文献标识码： A 中图分类号： TP273
（19）
（5） （6） （7） （8） （9） （10） （11） （12） （13）
T k
（20）
其中 x y z 为低轨卫星的三维位置；ẋ ẏ ż 为低轨卫星的三
w k， 维速度； b 为用户时钟的钟差； v k 分别为状态和观测噪声； f (×) 引用自文献[7]。
̂ P k/k - 1 = φ k - 1 P k - 1φT k - 1 + Qk - 1 ̂ kT P k = [ I m - k k H k ] P k/k - 1( I m - k k H k )T + k k R k-1 k
经典 Kalman 滤波方法应用于非线性系统时， 通过非线性 方程线性化后应用线性 Kalman 滤波进行次优滤波实现， 且要 求系统的数学模型和噪声统计已知， 而在实际应用中， 数学模 型或统计噪声是完全或部分未知的， 如果直接应用传统 Kalman 方法可能会引起滤波性能变坏， 甚至导致滤波发散[1-2]。为 了克服滤波发散问题， 文献 [4-6] 提出了基于 Sage-Husa[3] 的改 进自适应滤波方法， 但通过验证， 这些方法不能完全保证滤波 的稳定性。因此， 提出了一种新的非线性鲁棒扩展 Kalman 滤 波方法， 采用渐消记忆指数加权法和虚拟噪声补偿技术， 并在 状态噪声方差阵 Q 的估计中， 应用当前时刻的估计误差方差 代替前一时刻的估计误差方差， 从而达到滤波性能的稳定 性。以低轨卫星星载 GPS 定轨作为研究对象进行分析， 结果 验证了提出的方法较文献[6]中的算法鲁棒性强， 且滤波稳定。
tp273经典kalman滤波方法应用于非线性系统时通过非线性方程线性化后应用线性kalman滤波进行次优滤波实现且要求系统的数学模型和噪声统计已知而在实际应用中数学模型或统计噪声是完全或部分未知的如果直接应用传统kalman方法可能会引起滤波性能变坏甚至导致滤波发散12了克服滤波发散问题文献46提出了基于sagehusa进自适应滤波方法但通过验证这些方法不能完全保证滤波的稳定性
产生， 低轨卫星采用 J2 摄动模型。在仿真产生观测数据时， GPS 伪距和伪距率误差分别为 50 m (1σ ) 和 0.2 m/s (1σ ) 。下面 分别应用文献[6]提出的算法和本文的算法进行定轨计算。初 始状态变量的确定应用迭代最小二乘解算， P 的初值取为
其中 ξ k 中包含原始白噪声 w k 和 ϕ( x k k ) 在 x̂ k 处展开时的所 （1） （2）
η k 包括原有噪声 v k 和泰勒展开时 有高于二阶的高阶项 R(o) ； ξ k 和 η k 带有未知时变统计： 的所有高于二阶的高阶项 R(o) ； Eξ k = q k ， Cov[ξ k ξ j] = Q k δ kj
i i (z s - z u)( ż s - ż u)]/Ri} + Dρ̇ i , i=1， 2， …， n i s i s i s i s i s i s i s i s i s i s
（21）
（22） （23）
φ
)
r̂ k =Leabharlann (1 - d k - 1)r̂ k - 1 + d k - 1(z k - h( x̂ k/k - 1 k )) ̂ = (1 - d ) R ̂ R k k-1 k - 1 + d k - 1(ε ε - H k P k/k - 1 H )
w k、 测向量， h 为 m 维可微向量函数； m 维的高斯 v k 分别为 n、
白噪声， 它们彼此独立， 均值和协方差分别为：
Ew k = q Ev k = r Cov[w k w j] = Qδ kj Cov[v k v j] = Rδ kj
δ kj 是 Kronecker 函数。 其中， q、 Q、 r、 R 均未知时， 若采用经典 EKF 滤波， 将会导致滤
时变噪声统计估计器：
q̂ k = (1 - d k - 1)q̂ k - 1 + d k - 1( x̂ k - ϕ( x̂ k - 1 k - 1)) ̂ = (1 - d )Q ̂ Q k k-1 k-1 + d k - 1(k ε ε k + P k - φ k - 1 P
T k k k T k T k k T k-1 k-1
波误差变大， 甚至滤波发散。文献 [6] 提出了非线性鲁棒 Kalman 滤波， 其原理如下： 将式 （1） 中的 ϕ 围绕 k 时刻的状态估值 x̂ k 展开成泰勒级， 同时将观测方程中 h 在一步预测值 x̂ k/k - 1 处展开数， 进而得到：
xk + 1 = zk = ¶ϕ ¶ϕ x + (ϕ( x̂ k k ) x̂ ) + ξ k ¶x̂ k k ¶x̂ k k
Computer Engineering and Applications 计算机工程与应用
2011， 47 （3）
207
改进的非线性鲁棒 EKF 算法及其应用
2 吴志华 1， ， 丁杨斌 1， 申功勋 1 2 WU Zhihua1， ， DING Yangbin1， SHEN Gongxun1
1.北京航空航天大学 宇航学院， 北京 100083 2.中北大学 电子与计算机科学技术学院， 太原 030051 1.School of Aeronautics， Beijing University of Aeronautics and Astronautics， Beijing 100083， China 2.Institute of Electronic and Computer Science and Technology， North University of China， Taiyuan 030051， China WU Zhihua， DING Yangbin， SHEN Gongxun.Modified nonlinear robust EKF algorithm and its puter Engineering and Applications， 2011， 47 （3） ： 207-209. Abstract： Because of the uncertain noise statistics in nonlinear system， the filter error of the classical EKF （Extended Kalman Filter） will be enlarged even lead to filter divergence.To overcome this limitation， a novel method is proposed， it uses the method of fictitious noise compensating， and time-varying noise statistics can be estimated and compensated on line， so it improves the robust of nonlinear filter.At last， the proposed method is used in space-borne GPS orbit determination， the result shows that the novel method has better stability of filter and accuracy of orbit determination. Key words：nonlinear system； time-varying noise； robust EKF； space-born GPS orbit determination 摘 要： 非线性系统由于噪声统计的不确定性， 经常使得常规扩展 Kalman 滤波器滤波误差放大， 甚至引起滤波发散， 为此提出了
观测方程为： ρù Zk = é ê ú = h( X k k ) + v k ëρ̇ û 其中伪距、 伪距率分别表示为[8]：
ρi = Ri + C ´ b + Dρi （i=1， 2， 3， …， n） ρ̇ ={[( x - x u)( ẋ - ẋ u) + ( y - y u)( ẏ - ẏ u) +
（3） （4）
1
非线性鲁棒 Kalman 滤波
对于离散非线性随机系统：
x k + 1 = ϕ( x k k ) + w k z k = h( x k k ) + v k
¶h x + (h( x̂ k ) - ¶h x̂ k/k - 1) + η k k/k - 1 ¶x̂ k/k - 1 k ¶x̂ k/k - 1
T k T k
3 算法在星载 GPS 定轨中的应用 3.1 星载 GPS 定轨方程
在星载 GPS 低轨卫星定轨中， 状态和观测方程均为非线 性的， 可表示为： ìẊ = f ( X ) + w k í îZ k = h( X k k ) + v k 状态向量 X 取为：
X = [ x y z ẋ ẏ ż b]
2
提出的自适应非线性鲁棒扩展 Kalman 滤波算法
在上面的对状态和观测统计特性的方差估计中， 为了
3.2
算例分析
文中低轨卫星和 24 颗 GPS 卫星星历数据均由 STK 软件
达到无偏性， 在式 （12） 和 （14） 中引入了减法运算， 使得噪声 ̂ ̂ 统计二阶矩的估计 Q 和 R 分别失去半正定性和正定性， 从 而引起滤波发散
208
2011， 47 （3）
Eη k = r k ， Cov[η k η j] = R k δ kj
Computer Engineering and Applications 计算机工程与应用
虚拟噪声 ξ k 和 η k 分别补偿了线性化状态模型和线性化 观测模型的误差， 将非线性系统的滤波问题转化为线性滤 波问题。对式 （3） 和 （4） 运用传统 Kalman 滤波方法， 就得到带 未知噪声统计的非线性系统 （1） 、 （2） 的鲁棒扩展 Kalman 滤波 方程[6]：
k
Dρi 星到用户卫星间的几何距离； C 为光速； b 为接收机钟差； Dρ̇ i 为伪距率误差。 为伪距误差；
0 < b < 1 称为遗忘因子， 其取值一般在 0.950～0.995。对于慢
时变噪声统计， 应取较大的 b。该算法中对统计特性的估计为 无偏估计。
对于式 （19） 中连续的状态方程进行离散化后得到如式 （1） 的表示形式， 其与式 （19） 的观测方程一起组成如式 （1） 和 （2） 的离散非线性随机系统。
（14）
式中 ( x y z ) 和 ( ẋ ẏ ż ) 为第 i 颗 GPS 卫星的位置和速度，
i Ri = ( xis - x u)2 + ( yis - y u)2 + ( z s - z u)2 为观测到的第 i 颗 GPS 卫
其中： φk =
¶ϕ( x k ) | | | ，H k = ¶h | , d = (1 - b)/(1 - bk + 1) ， ¶x |x = x̂ ¶x |x = x̂ k/k - 1 k
ϕ 为 n 维可微向量函数； z k 为 m 维观 其中 x k 为 n 维状态向量，
基金项目： 国家部委创新项目 （No.7130617） 。 作者简介： 吴志华 （1972—） ， 女， 博士后， 副教授， 研究方向： 导航、 制导与控制。 收稿日期： 2009-09-04 修回日期： 2009-11-05
x̂ k = x̂ k/k - 1 + k k ε k x̂ k/k - 1 = ϕ( x̂ k - 1 k - 1) + q̂ k - 1 ε k = z k - h( x̂ k/k - 1 k ) - r̂ k - 1 ̂ )- 1 k k = P k/k - 1 H ( H k P k/k - 1 H + R k-1
一种采用虚拟噪声的补偿方法， 对时变的噪声统计进行在线估计且加以补偿， 提高了非线性滤波的鲁棒性。以星载 GPS 定轨为 研究对象， 应用该方法进行滤波定轨， 结果表明， 方法具有滤波稳定性和定轨结果的精确性。 关键词： 非线性系统； 时变噪声； 鲁棒 EKF； 星载 GPS 定轨 DOI： 10.3778/j.issn.1002-8331.2011.03.061 文章编号： 1002-8331 （2011） 03-0207-03 文献标识码： A 中图分类号： TP273
（19）
（5） （6） （7） （8） （9） （10） （11） （12） （13）
T k
（20）
其中 x y z 为低轨卫星的三维位置；ẋ ẏ ż 为低轨卫星的三
w k， 维速度； b 为用户时钟的钟差； v k 分别为状态和观测噪声； f (×) 引用自文献[7]。
̂ P k/k - 1 = φ k - 1 P k - 1φT k - 1 + Qk - 1 ̂ kT P k = [ I m - k k H k ] P k/k - 1( I m - k k H k )T + k k R k-1 k
经典 Kalman 滤波方法应用于非线性系统时， 通过非线性 方程线性化后应用线性 Kalman 滤波进行次优滤波实现， 且要 求系统的数学模型和噪声统计已知， 而在实际应用中， 数学模 型或统计噪声是完全或部分未知的， 如果直接应用传统 Kalman 方法可能会引起滤波性能变坏， 甚至导致滤波发散[1-2]。为 了克服滤波发散问题， 文献 [4-6] 提出了基于 Sage-Husa[3] 的改 进自适应滤波方法， 但通过验证， 这些方法不能完全保证滤波 的稳定性。因此， 提出了一种新的非线性鲁棒扩展 Kalman 滤 波方法， 采用渐消记忆指数加权法和虚拟噪声补偿技术， 并在 状态噪声方差阵 Q 的估计中， 应用当前时刻的估计误差方差 代替前一时刻的估计误差方差， 从而达到滤波性能的稳定 性。以低轨卫星星载 GPS 定轨作为研究对象进行分析， 结果 验证了提出的方法较文献[6]中的算法鲁棒性强， 且滤波稳定。
tp273经典kalman滤波方法应用于非线性系统时通过非线性方程线性化后应用线性kalman滤波进行次优滤波实现且要求系统的数学模型和噪声统计已知而在实际应用中数学模型或统计噪声是完全或部分未知的如果直接应用传统kalman方法可能会引起滤波性能变坏甚至导致滤波发散12了克服滤波发散问题文献46提出了基于sagehusa进自适应滤波方法但通过验证这些方法不能完全保证滤波的稳定性