CN100498373C - 一种sar运动补偿用sins/gps组合导航自适应降维滤波方法 - Google Patents

Abstract

一种SAR(合成孔径雷达)运动补偿用SINS(捷联惯导)/GPS组合导航系统自适应降维滤波方法，该方法离线建立不同飞行机动情况的全维STNS/GPS组合导航滤波模型库；载机机动性发生变化时在线计算每个状态变量可观测度；依据可观测度从大到小排列状态变量，选择包含三维位置和速度在内的前d维状态变量建立最小维数降维滤波模型，采用随机生成法确定状态估计方差矩阵中新增状态变量对应的元素。在载机机动性下一次发生变化之前采用所建立的降维滤波器实现组合导航。本发明利用状态变量可观测度与估计精度之间的对应关系构建自适应降维滤波模型，在保证滤波精度的同时显著提高了系统实时性。

Description

一种SAR运动补偿用SINS/GPS组合导航自适应降维滤波方法

技术领域

本发明涉及一种SAR(Synthetic Aperture Radar，合成孔径雷达)运动补偿用SINS(Strapdown Inertial Navigation System，捷联惯导)/GPS组合导航自适应降维滤波方法，特别是一种自适应降维滤波模型，特别适用于实时在线测量SAR成像系统天线相位中心运动参数等场合。

背景技术

SAR高分辨率遥感成像具有全天候成像、穿透成像、立体成像等优势，在战场侦察、全球实时预警、国土资源测绘、环境监测与保护、气候及自然灾害监测预警等领域有重要用途，是实现对地观测的军民两用高技术装备。SAR技术已经从事后成像方式向实时成像、实时传输方向发展。SAR成像的前提是必须在成像过程中精确测量天线相位中心的三维位置信息。由此可见，SAR实时成像迫切需要突破天线相位中心三维位置实时、高精度测量的技术瓶颈，称之为SAR实时运动补偿技术。

传统SAR运动补偿方案采用SINS/GPS组合导航系统测量天线相位中心的运动参数，SINS/GPS组合导航系统一般构建包含15维状态变量、6维观测变量的Kalman滤波模型。由于滤波模型维数较高，一步滤波的乘加次数为15³+6×15²，系统难以实时解算出SAR天线相位中心的位置信息，一般都是实时采集原始信息，事后处理得到SAR天线相位中心的位置信息并为事后成像SAR系统提供运动补偿参数。为了给SAR实时成像系统提供天线相位中心的三维位置信息，本发明提出了一种SAR运动补偿用SINS/GPS组合导航系统自适应降维滤波方法。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种精度高、实时性好的SINS/GPS组合导航自适应降维滤波方法，为SAR实时成像系统在线提供天线相位中心的三维位置信息。

本发明的技术解决方案是：一种SAR运动补偿用SINS/GPS组合导航系统自适应降维滤波方法，其特征在于包括以下步骤：A、离线建立全维(15维)滤波模型库，包括三种不同飞行机动情况下模型的状态方程、观测方程、系统噪声方差阵、量测噪声方差阵以及状态估计方差阵；三种滤波模型分别对应SAR载机匀速直线运动、匀加速直线运动、匀速圆周运动三种情况。B、系统启动工作，判断载机机动性发生变化，实时在线利用状态转移矩阵、观测矩阵以及基于奇异值分解原理的可观测度分析计算方法，计算15维状态变量的可观测度，依据可观测度从大到小重新排列状态变量。C、选择包含三维位置和速度在内的前d维状态变量构成维数最小的降维滤波模型，d等于三维位置和速度状态变量对应的最大排列序号；D、采用随机生成法确定新建降维滤波模型状态估计方差矩阵中新增状态变量对应的元素；E、启动并保持该降维滤波模型，直至SAR载机机动性再次发生变化。

本发明的原理是：SINS/GPS组合导航系统滤波模型中每个状态变量的滤波精度随着SAR载机飞行机动性的变化而变化，可观测度量化地表示了当前飞行机动情况下每个状态变量的滤波精度。SAR运动补偿用SINS/GPS组合导航自适应降维滤波方法在SAR载机机动性发生变化时实时计算每一个状态变量在当前机动状态下的可观测度，依据可观测度从大到小对状态变量进行排序。从大到小选择包含三维位置和速度在内的前d维状态变量以构建维数最小滤波模型，n等于三维位置和速度状态变量对应的最大排列序号。确定降维滤波模型对应的状态转移矩阵、系统噪声方差阵和状态估计方差阵，完成组合导航滤波；采用随机生成法确定状态估计方差矩阵中新增补状态变量对应的元素。SAR载机飞行机动性不发生变化时则保持当前的SINS/GPS组合导航滤波模型。本发明通过实时构建自适应降维滤波模型、在线删除滤波精度差的状态变量，大大减小了计算量；同时保存SAR成像所需要的高滤波精度状态变量。虽然在线实时计算状态变量的可观测度并构建降维滤波模型需要付出一定的计算代价，但考虑到SAR成像要求载机在绝大多数情况下保持匀速直线运动，见图1所示，SAR载机机动性发生变化的时间并不多，因此本发明利用较小的计算代价，即计算状态变量可观测度和构建降维滤波模型，达到了显著减小SAR运动补偿用SINS/GPS组合导航系统自适应降维滤波模型整体计算量的目的，而且保证了和全维滤波模型相同的滤波精度，满足了SAR实时成像系统的要求。

本发明与现有技术相比的优点在于：本发明采用了一种自适应降维滤波方法，使得SAR运动补偿用SINS/GPS组合导航系统在保证滤波精度的同时，显著减小计算量，为SAR实时成像系统奠定技术基础。这是由于：(1)与传统SINS/GPS组合导航系统滤波方法相比，由于本发明的滤波方法根据系统状态变量可观测度自适应在线取舍状态变量以组成降维滤波模型，抛弃滤波精度差的状态变量，完全不影响该滤波方法的精度；(2)显著减小计算量；由于滤波模型维数对系统计算量的影响极大，假设n、m分别为状态变量和观测变量的维数，一步滤波的乘加次数为n³+m×n²。本发明所提出的自适应降维滤波模型主要目的是减小SAR运动补偿用SINS/GPS组合导航滤波模型的维数，从而达到减小计算量的目的；(3)已经建立了SAR载机不同飞行机动性情况下的SINS/GPS组合导航滤波模型库，在线实时计算状态变量可观测度和构建降维滤波模型时只需要从中选择即可，大大简化了系统流程；(4)每次在线重构SINS/GPS组合导航滤波模型时，采用随机生成法确定状态估计方差矩阵中的新增补元素。随机生成法计算量小，且与上次滤波值方差的关联度高，有助于重构降维滤波模型的快速收敛。

附图说明

图1为本发明的SAR载机典型航路图；

图2为本发明的状态变量排序及选择流程示意图；

图3为本发明的方法工作流程图。

具体实施方式

如图3所示，本发明的具体步骤如下：

(1)首先构建全维SINS/GPS组合导航系统滤波模型库，包括：匀速直线运动、匀加速直线运动、匀速圆周运动三种模型。

a.匀速圆周运动全维SINS/GPS组合导航系统滤波模型：

15 维状态变量

$X\left(t\right)={\left[\begin{array}{ccccccccccccccc}{\mathrm{\φ}}_x& {\mathrm{\φ}}_y& {\mathrm{\φ}}_z& {\mathrm{\δv}}_x& {\mathrm{\δv}}_y& {\mathrm{\δv}}_z& \mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& {\▿}_x& {\▿}_t& {\▿}_z\end{array}\right]}^T$

SINS/GPS组合导航系统的状态方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

量测方程为：

Z(t)＝H(t)X(t)+V(t)

其中，F(t)为系统的状态转移矩阵： $F\left(t\right)={\left[\begin{array}{c}{F}_{N9\×15}\\ 0_{6\×15}\end{array}\right]}_{15\×15}$

F_N由以下一组微分方程的系数决定：

${\stackrel{\·}{\mathrm{\φ}}}_x=-\frac{{\mathrm{\δv}}_y}{R_M+h}+({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x}{R_N+h}\mathrm{tgL}){\mathrm{\φ}}_y-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}){\mathrm{\φ}}_z+{\mathrm{\ϵ}}_x$

${\stackrel{\·}{\mathrm{\φ}}}_y=\frac{{\mathrm{\δv}}_x}{R_N+h}-{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{L\δL}-({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x}{R_N+h}\mathrm{tgL}){\mathrm{\φ}}_x-\frac{v_y}{R_M+h}{\mathrm{\φ}}_z+{\mathrm{\ϵ}}_y$

${\stackrel{\·}{\mathrm{\φ}}}_z=\frac{{\mathrm{\δv}}_x}{R_N+h}\mathrm{tgL}+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}{\sec }^{2}L)\mathrm{\δL}+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}){\mathrm{\φ}}_x+\frac{v_y}{R_M+h}{\mathrm{\φ}}_y+{\mathrm{\ϵ}}_z$

$\mathrm{\δ}{\stackrel{\·}{v}}_x=f_y{\mathrm{\φ}}_z-f_z{\mathrm{\φ}}_y+(\frac{v_y}{R_M+h}\mathrm{tgL}-\frac{v_z}{R_M+h}){\mathrm{\δv}}_x+({2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x}{R_N+h}){\mathrm{\δv}}_y+$

$({2\mathrm{\ω}}_{\mathrm{ie}}\cos {\mathrm{Lv}}_y+\frac{v_x{v}_y}{R_N+h}{\sec }^{2}L+{2\mathrm{\ω}}_{\mathrm{ie}}\sin {\mathrm{Lv}}_z)\mathrm{\δL}-({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}){\mathrm{\δv}}_z+{\▿}_x$

$\mathrm{\δ}{\stackrel{\·}{v}}_y=f_z{\mathrm{\φ}}_x-f_x{\mathrm{\φ}}_z-2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x}{R_N+h}\mathrm{tgL}){\mathrm{\δv}}_x-\frac{v_z}{R_M+h}\mathrm{\δ}{v}_y-\frac{v_y}{R_M+h}{\mathrm{\δv}}_z$

$-({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}{\sec }^{2}L)v_x\mathrm{\δL}+{\▿}_y$

$\mathrm{\δ}{\stackrel{\·}{v}}_z=f_x{\mathrm{\φ}}_y-f_y{\mathrm{\φ}}_x-2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}){\mathrm{\δv}}_x+2\frac{v_y}{R_M+h}{\mathrm{\δv}}_z-{2\mathrm{\ω}}_{\mathrm{ie}}\sin {\mathrm{Lv}}_x\mathrm{\δL}+{\▿}_z$

$\mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δv}}_y}{R_M+h}$

$\mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{{\mathrm{\δv}}_x}{R_N+h}\sec L+\frac{v_x}{R_N+h}\sec \mathrm{LtgL\δL}$

$\mathrm{\δ}\stackrel{\·}{h}=\mathrm{\δ}{v}_z$

L，δL分别为纬度和纬度估计误差；λ，δλ分别为经度和经度估计误差；h，δh分别为高度和高度估计误差；v_x，δv_x，v_y，δv_y，v_z，δv_z分别为三纬速度和速度估计误差；φ_x，φ_y，φ_z分别为三维姿态估计误差；ε_x，ε_y，ε_z分别为三轴陀螺误差；

分别为三轴加速度计误差；f_x，f_y，f_z分别为三轴比力测量值；分别为SAR载机俯仰角、横滚角、偏航角；R_m，R_n，ω_ie分别为地球卯酉面半径、子午面半径、地球自转角速度。

G(t)为系统噪声转移矩阵： $G\left(t\right)={\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{9\×3}& 0_{9\×3}\end{array}\right]}_{15\×6}$ 其中：

W(t)为陀螺仪和加速度计的随机误差， $W\left(t\right)={\left[\begin{array}{cccccc}{w}_{\mathrm{\ϵx}}& w_{\mathrm{\ϵy}}& w_{\mathrm{\ϵz}}& w_{\▿x}& w_{\▿y}& w_{\▿z}\end{array}\right]}^T.$ 观测矩阵 $H\left(t\right)=\left[\begin{array}{c}{H}_v\\ H_p\end{array}\right],$  H_v＝[0_3×3 diag(111) 0_3×12]

H_p＝[0_3×6 diag(R_m R_ncosL 1)0_3×9]

观测矢量Z(t)＝[δv_x δv_y δv_z δL δλ δh]^T。

系统噪声方差阵Q是由 $W\left(t\right)={\left[\begin{array}{cccccc}{w}_{\mathrm{\ϵx}}& w_{\mathrm{\ϵy}}& w_{\mathrm{\ϵz}}& w_{\▿x}& w_{\▿y}& w_{\▿z}\end{array}\right]}^T.$ 各个元素的方差构成对角线元素的对角矩阵。量测噪声方差阵R是由Z(t)＝[δv_x δv_y δv_z δL δλ δh]^T各个元素的方差构成对角线元素的对角矩阵。状态估计方差阵的初始值定为15维的单位矩阵，在以后每一步组合滤波过程中按照常规Kalman滤波步骤实时更新该阵。

公式中用到的所有参数为当前时刻的导航参数，在每一步滤波中视为恒定值。

b.匀加速和匀速直线运动的SINS/GPS组合导航系统滤波模型与匀速圆周运动相比仅仅是构成状态转移矩阵F_N的微分方程不同。

匀加速直线运动全维SINS/GPS组合导航系统滤波模型是在匀速圆周运动滤波模型的基础上令 $v_z=0,$ 从而略去

中的

 2ω_ie sin Lv_zδL两项，

中的 $-\frac{v_z}{R_M+h}{\mathrm{\δv}}_y$ 项：

15维状态变量

$X\left(t\right)={\left[\begin{array}{ccccccccccccccc}{\mathrm{\φ}}_x& {\mathrm{\φ}}_y& {\mathrm{\φ}}_z& {\mathrm{\δv}}_x& {\mathrm{\δv}}_y& {\mathrm{\δv}}_z& \mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& {\▿}_x& {\▿}_t& {\▿}_z\end{array}\right]}^T$

SINS/GPS组合导航系统的状态方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

量测方程为：

Z(t)＝H(t)X(t)+V(t)

其中，F(t)为系统的状态转移矩阵： $F\left(t\right)={\left[\begin{array}{c}{F}_{N9\×15}\\ 0_{6\×15}\end{array}\right]}_{15\×15}$

F_N由以下一组微分方程的系数决定：

${\stackrel{\·}{\mathrm{\φ}}}_x=-\frac{{\mathrm{\δv}}_y}{R_M+h}+({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x}{R_N+h}\mathrm{tgL}){\mathrm{\φ}}_y-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}){\mathrm{\φ}}_z+{\mathrm{\ϵ}}_x$

${\stackrel{\·}{\mathrm{\φ}}}_y=\frac{{\mathrm{\δv}}_x}{R_N+h}-{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{L\δL}-({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x}{R_N+h}\mathrm{tgL}){\mathrm{\φ}}_x-\frac{v_y}{R_M+h}{\mathrm{\φ}}_z+{\mathrm{\ϵ}}_y$

${\stackrel{\·}{\mathrm{\φ}}}_z=\frac{{\mathrm{\δv}}_x}{R_N+h}\mathrm{tgL}+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}{\sec }^{2}L)\mathrm{\δL}+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}){\mathrm{\φ}}_x+\frac{v_y}{R_M+h}{\mathrm{\φ}}_y+{\mathrm{\ϵ}}_z$

$\mathrm{\δ}{\stackrel{\·}{v}}_x=f_y{\mathrm{\φ}}_z-f_z{\mathrm{\φ}}_y+\frac{v_y}{R_M+h}\mathrm{tgL\δ}{v}_x+({2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x}{R_N+h}){\mathrm{\δv}}_y+$

$({2\mathrm{\ω}}_{\mathrm{ie}}\cos {\mathrm{Lv}}_y+\frac{v_x{v}_y}{R_N+h}{\sec }^{2}L){\mathrm{\δ}}_L-({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}){\mathrm{\δv}}_z+{\▿}_x$

$\mathrm{\δ}{\stackrel{\·}{v}}_y=f_z{\mathrm{\φ}}_x-f_x{\mathrm{\φ}}_z-2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x}{R_N+h}\mathrm{tgL}){\mathrm{\δv}}_x-\frac{v_y}{R_M+h}{\mathrm{\δv}}_z$

$-({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}{\sec }^{2}L)v_x\mathrm{\δL}+{\▿}_y$

$\mathrm{\δ}{\stackrel{\·}{v}}_z=f_x{\mathrm{\φ}}_y-f_y{\mathrm{\φ}}_x-2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}){\mathrm{\δv}}_x+2\frac{v_y}{R_M+h}{\mathrm{\δv}}_z-{2\mathrm{\ω}}_{\mathrm{ie}}\sin {\mathrm{Lv}}_x\mathrm{\δL}+{\▿}_z$

$\mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δv}}_y}{R_M+h}$

$\mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{{\mathrm{\δv}}_x}{R_N+h}\sec L+\frac{v_x}{R_N+h}\sec \mathrm{LtgL\δL}$

$\mathrm{\δ}\stackrel{\·}{h}=\mathrm{\δ}{v}_z$

L，δL分别为纬度和纬度估计误差；λ，δλ分别为经度和经度估计误差；h，δh分别为高度和高度估计误差；v_x，δv_x，v_y，δv_y，v_z，δv_z分别为三纬速度和速度估计误差；φ_x，φ_y，φ_z分别为三维姿态估计误差；ε_x，ε_v，ε_z分别为三轴陀螺误差；

分别为三轴加速度计误差；f_x，f_y，f_z分别为三轴比力测量值；

分别为SAR载机俯仰角、横滚角、偏航角；R_m，R_n，ω_ie分别为地球卯酉面半径、子午面半径、地球自转角速度。

G(t)为系统噪声转移矩阵 $G\left(t\right)={\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{9\×3}& 0_{9\×3}\end{array}\right]}_{15\×6}$ 其中：

W(t)为陀螺仪和加速度计的随机误差， $W\left(t\right)={\left[\begin{array}{cccccc}{w}_{\mathrm{\ϵx}}& w_{\mathrm{\ϵy}}& w_{\mathrm{\ϵz}}& w_{\▿x}& w_{\▿y}& w_{\▿z}\end{array}\right]}^T.$ 观测矩阵 $H\left(t\right)=\left[\begin{array}{c}{H}_v\\ H_p\end{array}\right],$  H_v＝[0_3×3 diag(1 1 1) 0_3×12]，H_p＝[0_3×6 diag(R_m R_ncosL 1)0_3×9]

观测矢量Z(t)＝[δv_x δv_y δv_z δL δλ δh]^T。

系统噪声方差阵Q是由 $W\left(t\right)={\left[\begin{array}{cccccc}{w}_{\mathrm{\ϵx}}& w_{\mathrm{\ϵy}}& w_{\mathrm{\ϵz}}& w_{\▿x}& w_{\▿y}& w_{\▿z}\end{array}\right]}^T.$ 各个元素的方差构成对角线元素的对角矩阵。量测噪声方差阵R是由Z(t)＝[δv_x δv_y δv_z δL δλ δh]^T各个元素的方差构成对角线元素的对角矩阵。状态估计方差阵的初始值定为15维的单位矩阵，在以后每一步组合滤波过程中按照常规Kalman滤波步骤实时更新该阵。公式中用到的所有参数为当前时刻的导航参数，在每一步滤波中视为恒定值。

c.匀速直线运动全维SINS/GPS组合导航系统滤波模型是在匀速圆周运动滤波模型的基础上令v_z＝0，f_x＝f_y＝f_z＝0，从而略去

中的

2ω_ie sin Lv_zδL、f_yφ_z-f_zφ_y，

中的f_zφ_x-f_xφ_z、

中的f_xφ_y-f_yφ_x等项：

15维状态变量

$X\left(t\right)={\left[\begin{array}{ccccccccccccccc}{\mathrm{\φ}}_x& {\mathrm{\φ}}_y& {\mathrm{\φ}}_z& {\mathrm{\δv}}_x& {\mathrm{\δv}}_y& {\mathrm{\δv}}_z& \mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& {\▿}_x& {\▿}_t& {\▿}_z\end{array}\right]}^T$

SINS/GPS组合导航系统的状态方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

量测方程为：

Z(t)＝H(t)X(t)+V(t)

其中，F(t)为系统的状态转移矩阵： $F\left(t\right)={\left[\begin{array}{c}{F}_{N9\×15}\\ 0_{6\×15}\end{array}\right]}_{15\×15}$

F_N由以下一组微分方程的系数决定：

${\stackrel{\·}{\mathrm{\φ}}}_x=-\frac{{\mathrm{\δv}}_y}{R_M+h}+({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x}{R_N+h}\mathrm{tgL}){\mathrm{\φ}}_y-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}){\mathrm{\φ}}_z+{\mathrm{\ϵ}}_x$

${\stackrel{\·}{\mathrm{\φ}}}_y=\frac{{\mathrm{\δv}}_x}{R_N+h}-{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{L\δL}-({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x}{R_N+h}\mathrm{tgL}){\mathrm{\φ}}_x-\frac{v_y}{R_M+h}{\mathrm{\φ}}_z+{\mathrm{\ϵ}}_y$

${\stackrel{\·}{\mathrm{\φ}}}_z=\frac{{\mathrm{\δv}}_x}{R_N+h}\mathrm{tgL}+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}{\sec }^{2}L)\mathrm{\δL}+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}){\mathrm{\φ}}_x+\frac{v_y}{R_M+h}{\mathrm{\φ}}_y+{\mathrm{\ϵ}}_z$

$\mathrm{\δ}{\stackrel{\·}{v}}_x=\frac{v_y}{R_M+h}\mathrm{tgL\δ}{v}_x+({2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x}{R_N+h}){\mathrm{\δv}}_y+$

$({2\mathrm{\ω}}_{\mathrm{ie}}\cos {\mathrm{Lv}}_y+\frac{v_x{v}_y}{R_N+h}{\sec }^{2}L){\mathrm{\δ}}_L-({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}){\mathrm{\δv}}_z+{\▿}_x$

$\mathrm{\δ}{\stackrel{\·}{v}}_z=2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x}{R_N+h}\mathrm{tgL}){\mathrm{\δv}}_x-\frac{v_y}{R_M+h}{\mathrm{\δv}}_z$

$-({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}{\sec }^{2}L)v_x\mathrm{\δL}+{\▿}_y$

$\mathrm{\δ}{\stackrel{\·}{v}}_z=2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}){\mathrm{\δv}}_x+2\frac{v_y}{R_M+h}{\mathrm{\δv}}_z-{2\mathrm{\ω}}_{\mathrm{ie}}\sin {\mathrm{Lv}}_x\mathrm{\δL}+{\▿}_z$

$\mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δv}}_y}{R_M+h}$

$\mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{{\mathrm{\δv}}_x}{R_N+h}\sec L+\frac{v_x}{R_N+h}\sec \mathrm{LtgL\δL}$

$\mathrm{\δ}\stackrel{\·}{h}=\mathrm{\δ}{v}_z$

L，δL分别为纬度和纬度估计误差；λ，δλ分别为经度和经度估计误差；h，δh分别为高度和高度估计误差；v_x，δv_x，v_y，δv_y，v_z，δv_z分别为三纬速度和速度估计误差；φ_x，φ_y，φ_z分别为三维姿态估计误差；ε_x，ε_y，ε_z分别为三轴陀螺误差；分别为三轴加速度计误差；f_x，f_y，f_z分别为三轴比力测量值；分别为SAR载机俯仰角、横滚角、偏航角；R_m，R_n，ω_ie分别为地球卯酉面半径、子午面半径、地球自转角速度。

G(t)为系统噪声转移矩阵： $G\left(t\right)={\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{9\×3}& 0_{9\×3}\end{array}\right]}_{15\×6}$ 其中：

W(t)为陀螺仪和加速度计的随机误差， $W\left(t\right)={\left[\begin{array}{cccccc}{w}_{\mathrm{\ϵx}}& w_{\mathrm{\ϵy}}& w_{\mathrm{\ϵz}}& w_{\▿x}& w_{\▿y}& w_{\▿z}\end{array}\right]}^T.$ 观测矩阵 $H\left(t\right)=\left[\begin{array}{c}{H}_v\\ H_p\end{array}\right],$   H_v＝[0_3×3 diag(111)0_3×12]，H_p＝[0_3×6 diag(R_m R_ncosL 1)0_3×9]

观测矢量Z(t)＝[δv_x δv_y δv_z δL δλ δh]^T。

系统噪声方差阵Q是由 $W\left(t\right)={\left[\begin{array}{cccccc}{w}_{\mathrm{\ϵx}}& w_{\mathrm{\ϵy}}& w_{\mathrm{\ϵz}}& w_{\▿x}& w_{\▿y}& w_{\▿z}\end{array}\right]}^T$ 各个元素的方差构成对角线元素的对角矩阵。量测噪声方差阵R是由Z(t)＝[δv_x δv_y δv_z δL δλ δh]^T各个元素的方差构成对角线元素的对角矩阵。状态估计方差阵的初始值定为15维的单位矩阵，在以后每一步组合滤波过程中按照常规Kalman滤波步骤实时更新该阵。公式中用到的所有参数为当前时刻的导航参数，在每一步滤波中视为恒定值。

(2)其次，系统启动工作，判断载机机动性变化，辨识载机当前运动为匀速直线运动、或匀加速直线运动、或匀速圆周运动三种情况中的一种，实时在线计算可观测度，依据可观测度从大到小重新排列状态变量。

当三维加速度计输出小于规定的门限时判定为匀速直线运动，三维加速度计矢量和保持恒定常值时判定为匀加速直线运动，天向陀螺保持恒定常值时判定为匀速圆周运动。判断的周期与滤波周期相同，均为1秒。加速度计输出判断门限由系统选定加速度计的灵敏度决定。

在匀速直线运动、或匀加速直线运动、或匀速圆周运动的情况下计算全部15维状态变量的可观测度，令

$T=\left[\begin{array}{c}H\\ \mathrm{HF}\left(0\right)\\ \mathrm{HF}\left(1\right)F\left(0\right)\\ \·\·\·\\ \mathrm{HF}(k-1)F(k-2)...F\left(1\right)F\left(0\right)\end{array}\right]$

对T阵进行奇异值分解，得：

T＝U*S*V^T

其中：U＝[u₁ u₂ ...  u_m]，V＝[v₁ v₂ ... v_n]均为正交矩阵， $S=\left[\begin{array}{cc}{\mathrm{\Λ}}_{r\×r}& 0\\ 0& 0\end{array}\right]$ 为一个m×r阶的矩阵，其中Λ＝diag(σ₁ σ₂ ... σ_r)为对角阵，σ₁≥σ₂≥...≥σ_r≥0称为矩阵T的奇异值。根据奇异值的大小即可判断出其对应的状态变量是否可观测，哪些状态变量的可观测度高，哪些变量的可观测度低。

(3)依据可观测度从大到小对状态变量进行排序，取包含三维位置和三维速度在内的前d维状态变量构成最小维数滤波模型的状态向量。见图2所示。

(4)采用随机生成法确定新建降维滤波模型状态估计方差阵中新增状态变量对应的元素。

根据确定的状态变量构建降维滤波模型，其中采用当前SAR载机机动性对应的全维SINS/GPS组合导航滤波模型为基础，仅保留状态转移矩阵F(t)、系统噪声转移矩阵G(t)、观测矩阵H(t)中降维滤波模型所包含状态变量对应的元素；以最后一次估计降维滤波模型所包含状态变量的状态估计方差阵对应元素为均值，以该变量在上一次估计时刻的可观测度σ_prev与当前时刻可观测度σ_now的比值为方差 $\mathrm{\η}=\frac{{\mathrm{\σ}}_{\mathrm{prev}}}{{\mathrm{\σ}}_{\mathrm{now}}},$ 确定当前时刻滤波方差阵中对应该状态变量的元素，即新增状态变量的估计方差。

(5)在当前SAR载机机动性保持不变的情况下，保持上述降维滤波模型实现SAR运动补偿。当SAR载机机动性发生变化时则重新启动计算可观测度。

以上步骤循环进行，如图3所示，即可实现一种SAR运动补偿用SINS/GPS组合导航自适应降维滤波方法，在保证滤波精度的同时大大减小计算量。

Claims (5)

1、一种SAR运动补偿用SINS/GPS组合导航系统自适应降维滤波方法，其特征在于包括以下步骤：

A、离线建立全维，即15维滤波模型库，包括SAR载机匀速直线运动、匀加速直线运动、匀速圆周运动三种不同飞行机动情况下模型的状态方程、观测方程、系统噪声方差阵、量测噪声方差阵及状态估计方差阵；

B、系统启动工作，判断载机机动性变化，辨识载机当前运动为匀速直线运动、或匀加速直线运动、或匀速圆周运动三种情况中的一种，实时在线计算可观测度，依据可观测度从大到小重新排列状态变量；

C、选择包含三维位置和三维速度在内的前d维状态变量构成维数最小的降维滤波模型，d等于三维位置和速度状态变量对应的最大排列序号；

D、采用随机生成法确定新建降维滤波模型状态估计方差阵中新增状态变量对应的元素；

E、启动并保持该降维滤波模型，直至SAR载机机动性再次发生变化。

2、根据权利要求1所述的SAR运动补偿用SINS/GPS组合导航系统自适应降维滤波方法，其特征在于：所述的步骤B中辨识载机当前运动为匀速直线运动、或匀加速直线运动、或匀速圆周运动的方法为：当三维加速度计输出小于规定的门限时判定为匀速直线运动，三维加速度计矢量和保持恒定常值时判定为匀加速直线运动，天向陀螺保持恒定常值时判定为匀速圆周运动。

3、根据权利要求1所述的SAR运动补偿用SINS/GPS组合导航系统自适应降维滤波方法，其特征在于：所述的步骤B中的计算可观测度的方法为：利用状态转移矩阵、观测矩阵以及基于奇异值分解原理的可观测度分析计算方法，计算15维状态变量的可观测度。

4、根据权利要求1所述的SAR运动补偿用SINS/GPS组合导航系统自适应降维滤波方法，其特征在于：所述的步骤C中的降维滤波模型是依据可观测度从大到小对状态变量进行排序后，取包含三维位置信息、三维速度信息在内的前d维状态变量构成最小维数滤波模型。

5、根据权利要求1所述的SAR运动补偿用SINS/GPS组合导航系统自适应降维滤波方法，其特征在于：所述的步骤D中的随机生成法是以最后一次估计该变量时刻的状态估计方差矩阵对应元素为均值，按照该变量在上一次估计时刻的可观测度σ_prev与当前时刻可观测度σ_now的比值为方差 $\mathrm{\η}=\frac{{\mathrm{\σ}}_{\mathrm{prev}}}{{\mathrm{\σ}}_{\mathrm{now}}},$ 确定当前时刻滤波方差阵中对应该状态变量的元素。

Images