一种飞行器多个光流传感器与惯导组合导航方法

摘要

一种飞行器多个光流传感器与惯导组合导航方法，它有四大步骤：步骤一：将微机械三轴速率陀螺和三轴加速度计安装到飞行器上，组成捷联式惯性导航系统，建立惯导误差方程；步骤二：将3个光流传感器多点布置在飞行器上，建立光流传感器的量测方程；步骤三：根据光流传感器的量测方程，建立线性化的光流误差方程，作为组合导航系统的量测方程；步骤四：用扩展卡尔曼滤波器估计惯导误差，并使用此误差对惯导数据进行修正，得到更为精确的导航数据。本发明使用3个光流传感器、1套微机械三轴速率陀螺和1套微机械三轴加速度计，功耗小、成本低，便于在小型飞行器上安装布置，不对外辐射电磁信号，提高了飞行器的隐蔽性，是一种自主式组合导航方法。

主权项

1.一种飞行器多个光流传感器与惯导组合导航方法，其特征在于：该方法具体步骤如下：步骤一：将微机械三轴速率陀螺和三轴加速度计安装到飞行器上，组成捷联式惯性导航系统，建立惯导误差方程；导航坐标系选用ENU即东北天坐标系，该坐标系与地球表面固连，x轴指东，y轴指北，z轴指天；E、N、U三个方向的平台误差角方程分别为${\stackrel{·}{\phi }}_E={\phi }_N({\omega }_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)-{\phi }_U({\omega }_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})-\frac{{\mathrm{\delta V}}_N}{R_M+h}+\mathrm{\delta h}\frac{V_N}{{(R_M+h)}^2}-{ϵ}_E$${\stackrel{·}{\phi }}_N={-\phi }_E({\omega }_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)-{\phi }_U\frac{V_N}{R_M+h}-{\mathrm{\delta L\omega }}_{\mathrm{ie}}\sin L+\frac{{\mathrm{\delta V}}_E}{R_N+h}-\mathrm{\delta h}\frac{V_E}{{(R_N+h)}^2}-{ϵ}_N$                                                         （2）${\stackrel{·}{\phi }}_U={\phi }_E({\omega }_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})+{\phi }_N\frac{V_N}{R_M+h}+\mathrm{\delta L}({\omega }_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L)+\frac{{\mathrm{\delta V}}_E}{R_N+h}+\tan L$$-\mathrm{\delta h}\frac{V_E\tan L}{{(R_N+h)}^2}-{ϵ}_U$式中：${ϵ}_E=C_{11}{ϵ}_x^b+C_{21}{ϵ}_y^b+C_{31}{ϵ}_z^b$${ϵ}_N=C_{12}{ϵ}_x^b+C_{22}{ϵ}_y^b+C_{32}{ϵ}_z^b$${ϵ}_U=C_{13}{ϵ}_x^b+C_{23}{ϵ}_y^b+C_{33}{ϵ}_z^b$C_ij(i=1,2,3;j=1,2,3)为坐标变换矩阵中的子项，为导航坐标系到本体坐标系的变换矩阵：$C_n^b=\left(\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right)---\left(3\right)$其中L、λ、h分别为纬度、经度和高度，V_E、V_N、V_U分别为东向、北向和天向的速度，φ_E、φ_N、φ_U分别为东向、北向和天向的平台误差角，为三个陀螺的量测误差；ω_ie为地球自转角速度；R_M和R_N分别为地球的子午圈半径和卯酉圈半径；E、N、U三个方向的速度误差方程分别为$\delta {\stackrel{·}{V}}_E={\phi }_U{f}_N-{\phi }_N{f}_U+{\mathrm{\delta V}}_E\frac{V_N\tan L-V_U}{R_N+h}+{\mathrm{\delta V}}_N({2\omega }_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)$$-{\mathrm{\delta V}}_U({2\omega }_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})+\mathrm{\delta L}({2\omega }_{\mathrm{ie}}(V_N\cos L+V_U\sin L)+\frac{V_E{V}_N}{R_N+h}{\sec }^{2}L)$$+\mathrm{\delta h}\frac{V_E{V}_U-V_E{V}_N\tan L}{{(R_N+h)}^2}+{▿}_E$$\delta {\stackrel{·}{V}}_N={-\phi }_U{f}_E+{\phi }_E{f}_U-{2\mathrm{\delta V}}_E({\omega }_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)-{\mathrm{\delta V}}_N\frac{V_U}{R_M+h}-{\mathrm{\delta V}}_U\frac{V_N}{R_M+h}-$$\mathrm{\delta L}({2\omega }_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L)V_E+\mathrm{\delta h}(\frac{V_N{V}_U}{{(R_M+h)}^2}+\frac{V_E^2\tan L}{{(R_N+h)}^2})+{▿}_N$$\delta {\stackrel{·}{V}}_U={\phi }_N{f}_E-{\phi }_E{f}_N+{2\mathrm{\delta V}}_E({\omega }_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})+{\mathrm{\delta V}}_N\frac{{2V}_N}{R_M+h}-2{\mathrm{\delta LV}}_E{\omega }_{\mathrm{ie}}\sin L-$$\mathrm{\delta h}(\frac{V_E^2}{{(R_N+h)}^2}+\frac{V_N^2}{{(R_M+h)}^2})+{▿}_U---\left(4\right)$式中：${▿}_E=C_{11}{▿}_x^b+C_{21}{▿}_y^b+C_{31}{▿}_z^b$${▿}_E=C_{11}{▿}_x^b+C_{21}{▿}_y^b+C_{31}{▿}_z^b$${▿}_N=C_{12}{▿}_x^b+C_{22}{▿}_y^b+C_{32}{▿}_z^b$f_E、f_N、f_U为E、N、U三个方向的比力，为三个加速度计量测误差；E、N、U三个方向的位置误差方程分别为$\delta \stackrel{·}{L}=\frac{{\mathrm{\delta V}}_N}{R_M+h}-\mathrm{\delta h}\frac{V_N}{{(R_M+h)}^2}$$\delta \stackrel{·}{\lambda }=\frac{{\mathrm{\delta V}}_E}{R_N+h}\sec L+\mathrm{\delta L}\frac{V_E}{R_N+h}\sec L\tan L-\mathrm{\delta h}\frac{V_E\sec L}{{(R_N+h)}^2}---\left(5\right)$$\delta \stackrel{·}{h}={\mathrm{\delta V}}_U$于是，惯导误差方程写成：$\stackrel{·}{X}=\mathrm{FX}+\mathrm{Gw}---\left(6\right)$式中，$X={[\mathrm{\delta L},\mathrm{\delta \lambda },\mathrm{\delta h},{\mathrm{\delta V}}_E,{\mathrm{\delta V}}_N,{\mathrm{\delta V}}_U,{\phi }_E,{\phi }_N,{\phi }_U,{ϵ}_{\mathrm{cx}},{ϵ}_{\mathrm{cy}},{ϵ}_{\mathrm{cz}}{,ϵ}_{\mathrm{rx}},{ϵ}_{\mathrm{ry}},{ϵ}_{\mathrm{rz}},{▿}_x,{▿}_y,{▿}_z]}^T;$状态向量X共18维，其中δL、δλ、δh分别为纬度误差、经度误差和高度误差，δV_E、δV_N、δV_U分别为东向、北向和天向的速度误差，φ_E、φ_N、φ_U分别为东向、北向和天向的平台误差角，ε_cx，ε_cy，ε_cz为三个陀螺的随机常值偏差；ε_rx，ε_ry，ε_rz为三个陀螺的随机漂移；为三个加速度计的随机偏差；系统噪声为w=[ω_gx,ω_gy,ω_gz,ω_rx,ω_ry,ω_rz,ω_ax,ω_ay,ω_az]^T其中ω_gx，ω_gy，ω_gz为陀螺随机白噪声漂移；ω_rx，ω_ry，ω_rz为陀螺一阶马尔科夫驱动白噪声；ω_ax，ω_ay，ω_az为加速度计一阶马尔科夫驱动白噪声；系统噪声分配阵为$G=\left[\begin{array}{ccc}{0}_{6\times 3}& 0_{6\times 3}& 0_{6\times 3}\\ C_b^n& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}\end{array}\right]$F中的非零元素为$F_{\mathrm{1,3}}=\frac{{-V}_N}{{(R_M+h)}^2}$$F_{\mathrm{1,5}}=\frac{1}{R_M+h}$$F_{\mathrm{2,1}}=\frac{V_E\sec L\tan L}{R_N+h}$$F_{\mathrm{2,3}}=\frac{{-V}_E\sec L}{{(R_N+h)}^2}$$F_{\mathrm{2,4}}=\frac{\sec L}{R_N+h}$F_3,6=1$F_{\mathrm{4,1}}={2\omega }_{\mathrm{ie}}(V_N\cos L+V_U\sin L)+\frac{V_E{V}_N}{R_N+h}{\sec }^{2}L$$F_{\mathrm{4,3}}=\frac{V_E{V}_U-V_E{V}_N\tan L}{{(R_N+h)}^2}$$F_{\mathrm{4,4}}=\frac{V_N\tan L-V_U}{R_N+h}$$F_{\mathrm{4,5}}={2\omega }_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_N+h}$F_4,8=-f_U F_4,9=f_N F_4，16=C₁₁ F_4，17=C₂₁F_4，18=C₃₁$F_{\mathrm{5,1}}=-V_E({2\omega }_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L)$$F_{\mathrm{5,3}}=\frac{V_N{V}_U}{{(R_M+h)}^2}+\frac{V_E^2\tan L}{{(R_N+h)}^2}$$F_{\mathrm{5,4}}=-2({\omega }_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)$$F_{\mathrm{5,5}}=-\frac{V_U}{R_M+h}$$F_{\mathrm{5,6}}=-\frac{V_N}{R_M+h}$F_5,7=f_UF_5,9=-f_E F_5，16=C₁₂ F_5，17=C₂₂ F_5，18=C₃₂ F_6,1=-2V_Eω_iesinL$F_{\mathrm{6,3}}=-\frac{{V_E}^2}{{(R_N+h)}^2}-\frac{{V_N}^2}{{(R_M+h)}^2}$$F_{\mathrm{6,4}}={2\omega }_{\mathrm{ie}}\cos L+2\frac{V_E}{R_N+h}$$F_{\mathrm{6,5}}=\frac{{2V}_N}{R_M+h}$F_6,7=-f_N F_6,8=f_E F_6，16=C₁₃ F_6，17=C₂₃ F_6，18=C₃₃$F_{\mathrm{7,5}}=-\frac{1}{R_M+h}$$F_{\mathrm{7,8}}={\omega }_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L$$F_{\mathrm{7,9}}={-\omega }_{\mathrm{ie}}\cos L-\frac{V_E}{R_N+h}$F_7，10=-C₁₁ F_7，11=-C₂₁ F_7，12=-C₃₁ F_7，13=-C₁₁ F_7，14=-C₂₁ F_7，15=-C₃₁F_8,1=-ω_iesinL$F_{\mathrm{8,3}}=-\frac{V_E}{{(R_N+h)}^2}$$F_{\mathrm{8,4}}=\frac{1}{R_N+h}$$F_{\mathrm{8,7}}={-\omega }_{\mathrm{ie}}\sin L-\frac{V_E\tan L}{R_N+h}$F_8，10=-C₁₂ F_8，11=-C₂₂ F_8，12=-C₃₂ F_8，13=-C₁₂F_8，14=-C₂₂ F_8，15=-C₃₂$F_{\mathrm{9,1}}={\omega }_{\mathrm{ie}}\cos L+\frac{V_E{\sec }^{2}L}{R_N+h}$$F_{\mathrm{9,3}}=-\frac{V_E\tan L}{{(R_N+h)}^2}$$F_{\mathrm{9,4}}=\frac{\tan L}{R_N+h}$$F_{\mathrm{9,7}}={\omega }_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}$$F_{\mathrm{9,8}}=\frac{V_N}{R_M+h}$F_9，10=-C₁₃F_9，11=-C₂₃ F_9，12=-C₃₃ F_9，13=-C₁₃ F_9，14=-C₂₃ F_9，15=-C₃₃$F_{\mathrm{14,14}}=-\frac{1}{{\tau }_G}$$F_{\mathrm{15,15}}=-\frac{1}{{\tau }_G}$$F_{\mathrm{16,16}}=-\frac{1}{{\tau }_A}$$F_{\mathrm{17,17}}=-\frac{1}{{\tau }_A}$$F_{\mathrm{18,18}}=-\frac{1}{{\tau }_A};$步骤二：将3个光流传感器多点布置在飞行器上，建立光流传感器的量测方程；将3个光流传感器多点布置在飞行器上，在空间允许的情况下，各传感器间的距离要尽量远，并指向不同方向，这样做有利于提高后续的估计精度；其中，“多点布置”是指，光流传感器要安装在飞行器的不同位置，典型位置是头部、中间、尾部和翼梢；“距离要尽量远”是指，安装在头部、尾部或者翼梢的光流传感器，在不影响其它机载设备的情况下，要尽量靠近机体的最前端、最后端或者侧端，这样就保证了头部、尾部及翼梢光流传感器间的距离尽可能大些；在推导光流传感器的量测方程之前，先定义几个坐标系：导航坐标系(S_n):为了与惯导统一，选用ENU东北天坐标系，该坐标系与地球表面固连，x轴指东，y轴指北，z轴指天；本体坐标系(S_b):本体坐标系固连在MAV上，其原点在MAV的质心处，y轴指向MAV的前方，z轴沿MAV纵向对称面朝上，x轴按右手定则确定；光流传感器坐标系(S_f):光流传感器坐标系固连在光流传感器上，其原点在镜头的焦点处，z轴沿光轴方向指向外，x轴和y轴分别与测得的两个正交方向的光流重合；于是，光流传感器的量测值为：$f_f=\left(\begin{array}{c}{f}_x\\ f_y\end{array}\right)=\left(\begin{array}{c}\frac{(V_{\mathrm{nf}}{)}_{f,x}}{d_{\mathrm{fg}}}+{\left({\omega }_{\mathrm{nf}}\right)}_{f,y}\\ \frac{{\left(V_{\mathrm{nf}}\right)}_{f,y}}{d_{\mathrm{fg}}}-{\left({\omega }_{\mathrm{nf}}\right)}_{f,x}\end{array}\right)---\left(7\right)$这里,V_nf和ω_nf分别是光流传感器相对于导航坐标系的速度矢量和角速度矢量，下标f，x和f,y分别代表光流传感器坐标系中的x分量和y分量，d_fg为光流传感器的焦点沿z_f到地面的距离；令r_nb为S_b相对于S_n的位置矢量，r_bf为S_f相对于S_b的位置矢量，于是光流传感器的速度矢量表示为：$V_{\mathrm{nf}}=\frac{{\mathrm{dr}}_{\mathrm{nf}}}{\mathrm{dt}}=\frac{d}{\mathrm{dt}}(r_{\mathrm{nb}}+r_{\mathrm{bf}})=\frac{{\mathrm{dr}}_{\mathrm{nb}}}{\mathrm{dt}}+\frac{{\mathrm{dr}}_{\mathrm{bf}}}{\mathrm{dt}}---\left(8\right)$将速度矢量向S_f中投影：${\left(V_{\mathrm{nf}}\right)}_f=C_n^f\frac{d{\left(r_{\mathrm{nb}}\right)}_n}{\mathrm{dt}}+C_b^f(\frac{d{\left(r_{\mathrm{bf}}\right)}_b}{\mathrm{dt}}+{\left({\omega }_{\mathrm{nb}}\right)}_b\times {\left(r_{\mathrm{bf}}\right)}_b)$$=C_n^f{V}_n+C_b^f{({\omega }_{\mathrm{ib}}-{\omega }_{\mathrm{in}})}_b\times {\left(r_{\mathrm{bf}}\right)}_b---\left(9\right)$$=C_n^f{V}_n+C_b^f({\left({\omega }_{\mathrm{ib}}\right)}_b-C_n^b{({\omega }_{\mathrm{ie}}+{\omega }_{\mathrm{en}})}_n)\times {\left(r_{\mathrm{bf}}\right)}_b$本体坐标系到光流传感器坐标系的转换矩阵为：Y(μ)->X(η)$C_b^f=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \eta & \sin \eta \\ 0& -\sin \eta & \cos \eta \end{array}\right)\left(\begin{array}{ccc}\cos \mu & 0& -\sin \mu \\ 0& 1& 0\\ \sin \mu & 0& \cos \mu \end{array}\right)=\left(\begin{array}{ccc}\cos \mu & 0& -\sin \mu \\ \sin \eta \sin \mu & \cos \eta & \sin \eta \cos \mu \\ \cos \eta \sin \mu & -\sin \eta & \cos \eta \cos \mu \end{array}\right)---\left(10\right)$这里,μ和η是光流传感器的安装角，它们是光流传感器坐标系相对于本体坐标系的欧拉角，也就是说，将本体坐标系沿y_b轴转动角度μ，然后再沿x_b轴转动角度η，即得到光流传感器坐标系；由于μ和η是常值，故为常值矩阵；设z_f的方向矢量为k_f，即(k_f)_f=(0 0 1)^T，于是将k_f向S_n投影得：${\left(k_f\right)}_n=C_f^n{\left(k_f\right)}_f=C_b^n{C}_f^b{\left(k_f\right)}_f---\left(11\right)$z_f和z_n间夹角的余弦值为-(k_f)_n，z，于是：(k_f)_n，z=C₁₃cosηsinμ-C₂₃sinη+C₃₃cosηcosμ            （12）        =C₁₃T₃₁+C₂₃T₃₂+C₃₃T₃₃光流传感器沿其光轴方向到地面的距离为：$d_{\mathrm{fg}}=\left|\frac{{\left(r_{\mathrm{nf}}\right)}_{n,z}}{{\left(k_f\right)}_{n,z}}\right|$$=-\frac{{(r_{\mathrm{nb}}+r_{\mathrm{bf}})}_{n,z}}{{\left(k_f\right)}_{n,z}}$                                  （13）$=-\frac{{\left(r_{\mathrm{nb}}\right)}_{n,z}+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}{{\left(k_f\right)}_{n,z}}$$=-\frac{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}{{\left(k_f\right)}_{n,z}}$而${\left({\omega }_{\mathrm{nf}}\right)}_f={\left({\omega }_{\mathrm{nb}}\right)}_f$$=C_b^f{\left({\omega }_{\mathrm{nb}}\right)}_b$$=C_b^f{({\omega }_{\mathrm{ib}}-{\omega }_{\mathrm{in}})}_b---\left(14\right)$$=C_b^f{\left({\omega }_{\mathrm{ib}}\right)}_b-C_b^f{C}_n^b{\left({\omega }_{\mathrm{in}}\right)}_n$$=C_b^f{\left({\omega }_{\mathrm{ib}}\right)}_b-C_b^f{C}_n^b{({\omega }_{\mathrm{ie}}+{\omega }_{\mathrm{en}})}_n$于是，光流传感器的量测方程为$f_f=\left(\begin{array}{c}\frac{(V_{\mathrm{nf}}{)}_{f,x}}{d_{\mathrm{fg}}}+{\left({\omega }_{\mathrm{nf}}\right)}_{f,y}\\ \frac{{\left(V_{\mathrm{nf}}\right)}_{f,y}}{d_{\mathrm{fg}}}-{\left({\omega }_{\mathrm{nf}}\right)}_{f,x}\end{array}\right)$(15)；$=\left(\begin{array}{c}-\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{({\left({\omega }_{\mathrm{ib}}\right)}_b-C_n^b{({\omega }_{\mathrm{ie}}\times {\omega }_{\mathrm{en}})}_n)\times \left(r_{\mathrm{bf}}\right)}_b)}_x}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}+{(C_b^f{\left({\omega }_{\mathrm{ib}}\right)}_b-C_b^f{C}_n^b{({\omega }_{\mathrm{ie}}+{\omega }_{\mathrm{en}})}_n)}_y\\ -\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{({\left({\omega }_{\mathrm{ib}}\right)}_b-C_n^b{({\omega }_{\mathrm{ie}}+{\omega }_{\mathrm{en}})}_n)\times \left(r_{\mathrm{bf}}\right)}_b)}_y}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}-{(C_b^f{\left({\omega }_{\mathrm{ib}}\right)}_b-C_b^f{C}_n^b{({\omega }_{\mathrm{ie}}+{\omega }_{\mathrm{en}})}_n)}_x\end{array}\right)$步骤三：根据光流传感器的量测方程，建立线性化的光流误差方程，作为组合导航系统的量测方程；事实上，一个光流传感器可以同时测出正交方向上的两个光流分量，其量测输出可记为$f=\left(\begin{array}{c}{f}_x\\ f_y\end{array}\right)---\left(16\right)$求取光流误差方程为δf=H_fX+v(t)                    (17)v(t)为量测噪声，假设其为均值为0的白噪声，即E[v(t)]＝0，且E[v(t)v^T(τ)]=r_vδ(t-τ)，r_v为v(t)的方差强度阵；在推导光流量测方程的线化系数之前，首先对光流的量测方程进行合理的简化，光流的平动部分，即和基本上与是一个量级的，就一般飞行器来说，其量级大于10^-3s^-1，而|ω_ie|的量级为10^-5rad/s，量级不大于10^-5s^-1；另一方面，光流传感器是有噪声的，10^-5s^-1的数值会被量测噪声湮没，于是，光流量测方程（17）简化为：$f_f=\left(\begin{array}{c}-\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{{\left({\omega }_{\mathrm{ib}}\right)}_b\times \left(r_{\mathrm{bf}}\right)}_b)}_x}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}+{\left(C_b^f{\left({\omega }_{\mathrm{ib}}\right)}_b\right)}_y\\ -\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{{\left({\omega }_{\mathrm{ib}}\right)}_b\times \left(r_{\mathrm{bf}}\right)}_b)}_y}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}-{\left(C_b^f{\left({\omega }_{\mathrm{ib}}\right)}_b\right)}_x\end{array}\right)---\left(18\right)$现在求取典型安装位置和角度情况下的光流误差方程：1、安装在纵轴纵面内(r_bf)_b=(0,r_y,0)^T，μ＝π，于是：$C_b^f=\left(\begin{array}{ccc}\cos \mu & 0& -\sin \mu \\ \sin \eta \sin \mu & \cos \eta & \sin \eta \cos \mu \\ \cos \eta \sin \mu & -\sin \eta & \cos \eta \cos \mu \end{array}\right)=\left(\begin{array}{ccc}-1& 0& 0\\ 0& \cos \eta & -\sin \eta \\ 0& -\sin \eta & -\cos \eta \end{array}\right)=\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& {-T}_{22}\end{array}\right)---\left(19\right)$由于$C_n^b=\left(\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right),$于是，由式（13）得：${\left(k_f\right)}_f=C_b^n{C}_f^b{\left(k_f\right)}_f$$=\left(\begin{array}{ccc}{C}_{11}& C_{21}& C_{31}\\ C_{12}& C_{22}& C_{32}\\ C_{13}& C_{23}& C_{33}\end{array}\right)\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& -T_{22}\end{array}\right)\left(\begin{array}{c}0\\ 0\\ 1\end{array}\right)$$=\left(\begin{array}{ccc}{C}_{11}& C_{21}& C_{31}\\ C_{12}& C_{22}& C_{32}\\ C_{13}& C_{23}& C_{33}\end{array}\right)\left(\begin{array}{c}0\\ T_{23}\\ -T_{22}\end{array}\right)$$=\left(\begin{array}{c}{C}_{21}{T}_{23}-C_{31}{T}_{22}\\ C_{22}{T}_{23}-C_{32}{T}_{22}\\ C_{23}{T}_{23}-C_{33}{T}_{22}\end{array}\right)$于是，(k_f)_n，z=C₂₃T₂₃-C₃₃T₂₂                (20)由式（5），$C_b^n{\left(r_{\mathrm{bf}}\right)}_b=\left(\begin{array}{ccc}{C}_{11}& C_{21}& C_{31}\\ C_{12}& C_{22}& C_{32}\\ C_{13}& C_{23}& C_{33}\end{array}\right)\left(\begin{array}{c}0\\ r_y\\ 0\end{array}\right)=\left(\begin{array}{c}{C}_{21}{r}_y\\ C_{22}{r}_y\\ C_{23}{r}_y\end{array}\right),$于是${\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z=C_{23}{r}_y---\left(21\right)$而，$C_n^f{V}_n=C_b^f{C}_n^b{V}_n$$=\begin{array}{c}\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& -T_{22}\end{array}\right)\end{array}\left(\begin{array}{ccc}{C}_{11}& C_{21}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right)\left(\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right)$$=\begin{array}{c}\end{array}\left(\begin{array}{ccc}-C_{11}& -C_{12}& {-C}_{13}\\ C_{21}{T}_{22}+C_{31}{T}_{23}& C_{22}{T}_{22}+C_{32}{T}_{23}& C_{23}{T}_{22}+C_{33}{T}_{23}\\ C_{21}{T}_{23}-C_{31}{T}_{22}& C_{22}{T}_{23}-C_{32}{T}_{22}& C_{23}{T}_{23}-C_{33}{T}_{22}\end{array}\right)\left(\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right)---\left(22\right)$$=\left(\begin{array}{c}{-C}_{11}{V}_E-C_{12}{V}_N-C_{13}{V}_U\\ (C_{21}{T}_{22}+C_{31}+T_{23})V_E+(C_{22}{T}_{22}+C_{32}{T}_{23})V_N+(C_{23}{T}_{22}+C_{33}{T}_{23})V_U\\ (C_{21}{T}_{23}-C_{31}{T}_{22})V_E+(C_{22}{T}_{23}-C_{32}{T}_{22})V_N+(C_{23}{T}_{23}-C_{33}{T}_{22})V_U\end{array}\right)$$C_b^f{\left({\omega }_{\mathrm{ib}}\right)}_b\times {\left(r_{\mathrm{bf}}\right)}_b=\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& -T_{22}\end{array}\right)\left(\begin{array}{ccc}0& {-\omega }_{\mathrm{ib},z}^b& {\omega }_{\mathrm{ib},y}^b\\ {\omega }_{\mathrm{ib},z}^b& 0& {-\omega }_{\mathrm{ib},x}^b\\ {-\omega }_{\mathrm{ib},y}^b& {\omega }_{\mathrm{ib},x}^b& 0\end{array}\right)\left(\begin{array}{c}0\\ r_y\\ 0\end{array}\right)$                                       (23)$=\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& {-T}_{22}\end{array}\right)\left(\begin{array}{c}{-\omega }_{\mathrm{ib},z}^b{r}_y\\ 0\\ {\omega }_{\mathrm{ib},x}^b{r}_y\end{array}\right)=\left(\begin{array}{c}{\omega }_{\mathrm{ib},z}^b{r}_y\\ T_{23}{\omega }_{\mathrm{ib},x}^b\\ -T_{22}{\omega }_{\mathrm{ib},x}^b{r}_y\end{array}{r}_y\right)$$C_b^f{\left({\omega }_{\mathrm{ib}}\right)}_b=\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& {-T}_{22}\end{array}\right)\left(\begin{array}{c}{\omega }_{\mathrm{ib},z}^b\\ {\omega }_{\mathrm{ib},y}^b\\ {\omega }_{\mathrm{ib},z}^b\end{array}\right)=\left(\begin{array}{c}{-\omega }_{\mathrm{ib},x}^b\\ T_{22}{\omega }_{\mathrm{ib},y}^b+T_{23}{\omega }_{\mathrm{ib},z}^b\\ T_{23}{\omega }_{\mathrm{ib},y}^b-T_{22}{\omega }_{\mathrm{ib},z}^b\end{array}\right)---\left(24\right)$将式（20）~（24）代入光流公式（18）得：$f_f=\left(\begin{array}{c}-\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{{\left({\omega }_{\mathrm{ib}}\right)}_b\times \left(r_{\mathrm{bf}}\right)}_b)}_x}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}+{\left(C_b^f{\left({\omega }_{\mathrm{ib}}\right)}_b\right)}_y\\ -\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{{\left({\omega }_{\mathrm{ib}}\right)}_b\times \left(r_{\mathrm{bf}}\right)}_b)}_y}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}-{\left(C_b^f{\left({\omega }_{\mathrm{ib}}\right)}_b\right)}_x\end{array}\right)---\left(25\right)$展开并写成分量形式：$f_x=-\frac{(C_{23}{T}_{23}-C_{33}{T}_{22})}{h+C_{23}{r}_y}\times \left(\begin{array}{c}{-C}_{11}{V}_E-C_{12}{V}_N-\\ C_{13}{V}_U+{\omega }_{\mathrm{ib},z}^b{r}_y\end{array}\right)+(T_{22}{\omega }_{\mathrm{ib},y}^b+T_{23}{\omega }_{\mathrm{ib},z}^b)---\left(26\right)$$f_y=-\frac{(C_{23}{T}_{23}-C_{33}{T}_{22})}{h+C_{23}{r}_y}\times \left(\begin{array}{c}(C_{21}{T}_{22}+C_{31}{T}_{23})V_E-(C_{22}{T}_{22}+C_{32}{T}_{23})V_N\\ +(C_{23}{T}_{22}+C_{33}{T}_{23})V_U+{T_{23}\omega }_{\mathrm{ib},x}^b{r}_y\end{array}\right)+{\omega }_{\mathrm{ib},x}^b---\left(27\right)$对于飞机来说，其水平速度一般要远大于其垂直速度，故上式简化为：$f_x=-\frac{(C_{23}{T}_{23}-C_{33}{T}_{22})}{h+C_{23}{r}_y}\times ({-C}_{11}{V}_E-C_{12}{V}_N+{\omega }_{\mathrm{ib},z}^b{r}_y)+(T_{22}{\omega }_{\mathrm{ib},y}^b+T_{23}{\omega }_{\mathrm{ib},z}^b)---\left(28\right)$$f_y=-\frac{(C_{23}{T}_{23}-C_{33}{T}_{22})}{h+C_{23}{r}_y}\times \left(\begin{array}{c}(C_{21}{T}_{22}+C_{31}{T}_{23})V_E+(C_{22}{T}_{22}+C_{32}{T}_{23})V_N\\ +T_{23}{\omega }_{\mathrm{ib},x}^b{r}_y\end{array}\right)+{\omega }_{\mathrm{ib},x}^b---\left(29\right)$方（28）（29）未考虑任何误差，而实际系统中总存在各种误差，所以实际的光流应由下述方程确定，以式（28）表示的x向光流为例：$f_x+{\mathrm{\delta f}}_x=-\frac{({\hat{C}}_{23}{T}_{23}-{\hat{C}}_{33}{T}_{22})}{h+\mathrm{\delta h}+{\hat{C}}_{23}{r}_y}\times (-{\hat{C}}_{11}(V_E+{\mathrm{\delta V}}_E)-{\hat{C}}_{12}(V_N+{\mathrm{\delta V}}_N)+({\omega }_{\mathrm{ib},z}^b+{\mathrm{\delta \omega }}_{\mathrm{ib},z}^b)r_y)+---\left(30\right)$$(T_{22}({\omega }_{\mathrm{ib},y}^b+{\mathrm{\delta \omega }}_{\mathrm{ib},y}^b)+T_{23}({\omega }_{\mathrm{ib},z}^b+{\mathrm{\delta \omega }}_{\mathrm{ib},z}^b))$式（30）中，由下式确定：${\hat{C}}_n^b=C_n^b(I+{\Phi }_{\times }^n)$$=\left(\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right)\left(\begin{array}{ccc}1& {-\phi }_U& {\phi }_N\\ {\phi }_U& 1& {-\phi }_E\\ {-\phi }_N& {\phi }_E& 1\end{array}\right)---\left(31\right)$$=\left(\begin{array}{ccc}{C}_{11}+{\phi }_U{C}_{12}-{\phi }_N{C}_{13}& {-\phi }_U{C}_{11}+C_{12}+{\phi }_E{C}_{13}& {\phi }_N{C}_{11}-{\phi }_E{C}_{12}+C_{13}\\ C_{21}+{\phi }_U{C}_{22}-{\phi }_N{C}_{23}& {-\phi }_U{C}_{21}+C_{22}+{\phi }_E{C}_{23}& {\phi }_N{C}_{21}-{\phi }_E{C}_{22}+C_{23}\\ C_{31}+{\phi }_U{C}_{32}-{\phi }_N{C}_{33}& {-\phi }_U{C}_{31}+C_{32}+{\phi }_E{C}_{33}& {\phi }_N{C}_{31}+{\phi }_E{C}_{32}+C_{33}\end{array}\right)$（30）减去式（28），并略去高阶小项，即得到光流的误差方程：δf_x=H_fx[δh δV_E δV_N φ_E φ_N φ_U ε_cx ε_cy ε_cz ε_rx ε_ry ε_rz]^T    (32)H_fx是一个1×12的行阵，它的各个子项都很复杂，这里只给出相对简单的第一项，其它的不再一一列出，$H_{\mathrm{fx}}\left(\mathrm{1,1}\right)=\frac{1}{{(h+C_{23}{r}_y)}^2}\left(\begin{array}{c}(C_{23}{T}_{23}-C_{33}{T}_{22}{)\omega }_{\mathrm{ib},z}^b{r}_y+(C_{33}{T}_{22}{C}_{11}-C_{23}{T}_{23}{C}_{11})V_E+\\ (C_{33}{T}_{22}{C}_{12}-C_{23}{T}_{23}{C}_{12})V_N\end{array}\right)$同理得到δf_y=H_fy[δh δV_E δV_N φ_E φ_N φ_U ε_cx ε_cy ε_cz ε_rx ε_ry ε_rz]^T    (33)合并可得光流误差方程为：δ_f=H_f[δh δV_E δV_N φ_E φ_N φ_U ε_cx ε_cy ε_cz ε_rx ε_ry ε_rz]^T    (34)