[发明专利]一种基于非线性滤波的电控罗经初始对准方法

权利要求书

1.一种基于非线性滤波的电控罗经初始对准方法，其特征在于：电控 罗经非线性初始对准模型，首先，建立电控罗经采用固定指北式惯导机械 编排，得到包括陀螺误差、加速度计误差、平台失准角、速度误差、位置 误差诸因子的21维状态变量的误差模型，然后进行降维处理至15维，取 α、β、γ、δV_x、δV_y作为误差状态变量，得到系统对准的非线性误差 模型如下：

α·=-wiesinγcosφ+βwiesinφ-δVx/Re+ϵx+wxβ·=(1-cosγ)wiecosφ-αwiesinφ+δVx/Re+ϵy+wyγ·=(-βsinγ+αcosγ)wiecosφ+tanφδVx/Re+ϵz+wzδV·x=-(cosγ-1)ax+aysinγ-g(βcosγ+αsinγ)+2wieδVysinφ+▿y+wδVxδV·y=-sinγax+(cosγ-1)ay+g(-βsinγ+αcosγ)-2wieδVxsinφ+▿x+wδVy---(1)]]>

将上式改写为如下形式：

X·=g(X)+Bω---(2)]]>

式中：x＝[αβγδV_xδV_y]^T，w＝[w_x w_y w_z w_δVx w_δVy]^T是陀螺和加速度计的 随机白噪声；B＝I_5×5；其中R_e是地球半径；ω_ie是地球转动角速度；α、β、γ 是平台误差角在导航坐标系三个轴上的投影；δV_x、δV_y分别是纬度和东北 向速度误差；ε_x、ε_y、ε_z是陀螺常值误差在当地地理坐标系oxyz中的投 影，是水平加速度计常值偏置；

g(·)是非线性的，对其进行离散化，设采样间隔为Δt，将X(t+Δt)在X(t) 处进行泰勒展开，得到

X(t+Δt)=X(t)+g(x)Δt+∂g(x)∂xg(x)(Δt)22+...+Bw(t)---(3)]]>

令X(t+Δt)＝x_k+1，X(t)＝x_k，ω(t)＝ω_k，根据精度要求，忽略展开级 数的二阶及二阶以上的小量，得到系统的离散方程为：

x_k+1＝x_k+g(x_k)Δt+Bω_k            (4)

式(4)为电控罗经的误差模型；

建UKF滤波状态方程，电控罗经UKF滤波采用输出校正滤波器结构， 状态方程为电控罗经的非线性误差模型方程(4)；

建UKF滤波量测方程，设磁传感器输出的姿态分别为纵摇R_clj、横摇 P_clj、航向H_clj，罗经输出的姿态角为R_INS，P_INS，H_INS，于是可近似认为得到姿 态误差角的观测为

α＝R_clj-R_INS

β＝P_clj-P_INS                     (5)

γ＝H_clj-H_INS

对式(5)进行变形，并令z＝[R_clj P_clj H_clj]^T，有

z=RcljPcljHclj=100010001αβγ+RINSPINSHINS+Wclj---(6)]]>

其中W_clj为磁传感器的误差观测噪声，对准的短时间内可认为是零均 值白噪声；将测量方程简记为

z_k＝h(x_k)+v_k                      (7)

其中v_k为观测噪声。

按电控罗经的误差模型方程和观测噪声方程式(4)和(7)定义的系统， 改写为下式：

x(k+1)＝f[x(k)，u(k)，w(k)]       (8)

z(k)＝h[x(k)，v(k)]               (9)

在UKF滤波中，由于具有噪声项，需要对状态进行扩维处理，令 x^a＝[x^T w^T v^T]^T，具体UKF滤波流程如下：

状态初始条件为

x^0=E(x0)]]>

P0=E{(x0-x^0)(x0-x^0)T}]]>

x^0a=E(xa)=x0T00T---(10)]]>

P0a=E{(x0a-x^0a)(x0a-x^0a)T}=P0000Rw000Rv]]>

对于k∈{1，2，....∞}，计算重新构建的sigma点：

χk-1a=x^k-1ax^k-1a+ξPk-1ax^k-1a-ξPk-1a---(11)]]>

UKF的预测方程是：

χk|k-1a=f(χk-1x,uk-1,χk-1w)]]>

x^k-=Σi=02LWimχi,k|k-1x]]>

                           (12)

Pk-=Σi=02LWic[χi,k|k-1x-x^k-[χi,k|k-1x-x^k-]T]]>

Zk|k-1=h(χk|k-1a,χk|k-1v)]]>

z^k-=Σi=02LWimZi,k|k-1]]>

UKF的更新方程为：

Pz^kz^k=Σi=02LWic[Zi,k|k-1-z^k-][Zi,k|k-1-z^k-]T]]>

Pxkzk=Σi=02LWic[χi,k|k-1-x^k-][Zi,k|k-1-z^k-]T]]>

                             (13)

κk=PxkzkPz^kz^k-1]]>

x^k=x^k-+κk(zk-z^k-)]]>

Pk=Pk--κkPz^kz^kκkT]]>

其中x^a＝[x^T w^T v^T]^T，χ^a＝[(χ^x)^T (χ^w)^T (χ^v)^T]^T，式(11)中的ζ取值表sigma点采 样方法，即UT变换的方法；其中加权系数W_i的计算式为

W0m=λ/(L+λ)]]>

                              (14)

W0c=λ/(L+λ)+(1-τ2+η)]]>

Wim=Wic=1/{2(L+λ)}]]>

其中τ是一个小正数(通常取为1＞τ≥1e^-4)，决定了 sigma点在周围的散布，λ＝τ²(L+k)-L是一个标度因子，k也是一个 标度因子，常取为0或3-L，η是对针状态向量x的分布所加的一个参量， η取2，是矩阵平方根的第i列。