本发明提供一种基于卡尔曼滤波模型的三线阵影像POS辅助对地定位方法，包括建立局部坐标系作为统一的基准坐标系，将导航数据信息转换到基准坐标系中；将飞行获取的所用影像纠正到平均地面高的平面，形成中间影像，开展影像匹配提取影像连接点；根据外业测量获取的地面控制点信息，指定控制点影像位置；建立卡尔曼平差模型，根据POS系统获取的参数，建立运动方程；基于三线阵相机连续获取影像的过程符合动力学原理，根据运动方程，进行三线阵相机卡尔曼滤波的平差迭代求解计算，得到误差校正后的位置姿态数据。本发明可以更快速、更准确地求解影像的定位参数，采用本方法可实现稀少控制点下的高精度定位，有效填补国产三线阵相机高精度数据处理空白。
1.一种基于卡尔曼滤波模型的三线阵影像POS辅助对地定位方法，其特征在于，包括如下步骤：步骤1，建立局部坐标系作为统一的基准坐标系，将导航数据信息转换到基准坐标系中；
步骤2，根据POS记录的位置姿态参数和相机参数将飞行获取的所用影像纠正到平均地面高的平面，形成中间影像，记为D0影像；
步骤3，使用步骤2生产的D0影像开展影像匹配，提取影像连接点；
步骤4，根据外业测量获取的地面控制点信息，在D0影像中，指定控制点影像位置；
步骤5，建立卡尔曼平差模型，联合建立三线阵POS记录数据和影像扫描行之间的关系，根据POS系统获取的参数，建立K状态下的运动方程；所述POS系统获取的参数为航拍状态矢量，包括相机位置矢量、速度矢量和加速度矢量，以及相机姿态矢量、相应变化率矢量和加速度变化率矢量；
步骤6，基于三线阵相机连续获取影像的过程符合动力学原理，根据运动方程，利用K状态下参数进行预估，计算K+1状态下状态参数的先验预测值X￣(K+1)；
步骤7，根据运动方程，计算K+1状态的先验预测值X￣(K+1)的误差协方差矩阵P￣(K+1)；
步骤8，利用测量方程，建立K+1状态下先验预测值X￣(K+1)和测量值Z(K+1)之间的函数关系，并获取测量矩阵H(K+1)；
步骤9，根据运动方程中获得的当前状态误差协方差矩阵和测量矩阵计算当前状态的卡尔曼增益Kg(K+1)；
步骤10，将增益Kg(K+1)带入到更新方程中，对步骤6和步骤7所得结果进行修正，获取第K+1状态下状态矢量最佳后验估计值X(K+1)和控制点测量值Z(K+1)，同时计算第K+1状态下矢量的误差协方差矩阵P(K+1)；
步骤11，判断终止条件，若不满足转到步骤6继续迭代，否则迭代结束，将误差校正后的位置姿态数据输出作为最终结果。
2.如权利要求1所述的一种基于卡尔曼滤波模型的三线阵影像POS辅助对地定位方法，其特征在于：步骤5中POS系统获取的参数，为航拍状态矢量包括相机位置矢量[Xs，Ys，Zs]、速度矢量[Xs'，Ys'，Zs']和加速度矢量[Xs”，Ys”，Zs”]，相机姿态矢量 相应变化率矢量 和加速度变化率矢量设卡尔曼运动方程为：
式中， 是指由K状态下状态参数的后验估计值X(K)计算得到的K+1状态下状态参数的理论真实值；W(K+1)是白噪声，G(K+1)是噪声系数；
A(K+1)是指由K状态到K+1状态的转移矩阵，其形式为A(K+1)＝diag[a,a,a,a,a,a]，diag[]表示分块对角阵，其中a为3×3的上三角阵，形式为：其中，t表示由K状态到K+1状态之间的时间间隔，数值从POS记录数据中读取。
3.如权利要求2所述的一种基于卡尔曼滤波模型的三线阵影像POS辅助对地定位方法，‑其特征在于：步骤6中，K+1状态下状态矢量的先验预测值X￣(K+1)预测值计算公式为，‑X￣(K+1)＝A(K+1)×X(K)+(G(K+1)×W(K+1))‑
式中，X(K+1)是指由K状态下状态参数的后验估计值预测得到的K+1状态下状态参数的先验预测值。
4.如权利要求3所述的一种基于卡尔曼滤波模型的三线阵影像POS辅助对地定位方法，‑ ‑ ‑其特征在于：步骤7中，K+1状态的先验预测值X (K+1)的误差协方差矩阵P (K+1)，是X (K+1)与‑之差的协方差矩阵，由 的计算公式和X (K+1)的计算公式根据协方差矩阵计算方式得到如下形式，计算K+1状态的误差协方差矩阵，计算公式为，‑ T
P(K+1)＝A(K+1)×P(K)×A(K+1) +Q(K+1)‑ ‑
式中，P (K+1)是X (K+1)对应的误差协方差矩阵；P(K)是K状态下X(K)对应的误差协方差矩阵，采用卡尔曼滤波的结果；Q(K)是噪声的协方差矩阵。
5.如权利要求4所述的一种基于卡尔曼滤波模型的三线阵影像POS辅助对地定位方法，‑其特征在于：步骤8中，利用测量方程，建立K+1状态下先验预测值X (K+1)与控制点坐标之间的函数关系，所述测量方程为：‑
Z(K+1)＝h(X(K+1))+V(K+1)式中，Z(K+1)是K+1状态下引入的控制点地面坐标测量值，具体形式为：K+1 K+1 K+1 K+1 K+1 K+1 K+1 K+1 T K+1 K+1Z(K+1)＝[x1 ，y1 ，x2 ，y2 ，x3 ，y3 ，…xn ，yn ] ，n为控制点总数，xi 和yi 表示K+1状态下引入的第i个控制点的像片坐标(i∈[1,n])；
h(X￣(K+1))为测量方程函数矩阵，h(X￣(K+1))＝[h1(X￣(K+1)),h2(X￣(K+1)),…,hn(XT￣(K+1))]，hi(X￣(K+1))是个非线性函数，采用摄影测量构像方程。
6.如权利要求5所述的一种基于卡尔曼滤波模型的三线阵影像POS辅助对地定位方法，其特征在于：步骤9中，卡尔曼增益Kg(K+1)的计算公式如下：T T
Kg(K+1)＝P￣(K+1)×H(K+1) /(H(K+1)×P￣(K+1)×H(K+1) +R(K+1))式中，
P￣(K+1)是X￣(K+1)对应的误差协方差矩阵；
H(K+1)为线性化的测量矩阵，H(K+1) 是对应的转置矩阵；
7.如权利要求6所述的一种基于卡尔曼滤波模型的三线阵影像POS辅助对地定位方法，其特征在于：步骤10中，更新方程所用的计算公式为，X(K+1)＝X￣(K+1)+Kg(K+1)×(Z(K+1)‑(H(K+1)×X￣(K+1)+V(K+1)))P(K+1)＝P￣(K+1)–Kg(K+1)×H(K+1)×P￣(K+1)式中，
X(K+1)是该状态下参数的最佳后验估计值，是X￣(K+1)的新值；
P(K+1)是X(K+1)对应状态下参数的误差协方差矩阵，是P￣(K+1)的新值。
基于卡尔曼滤波模型的三线阵影像POS辅助对地定位方法
技术领域
[0001] 本发明属于遥感测绘技术领域，特别是涉及一种针对国产三线阵相机获取的影像进行的高精度对地定位方法。
背景技术
[0002] 三线阵成像系统是一种快速获取地面影像信息的设备，与传统固定框幅成像系统和多视倾斜成像系统相比，数据获取效率高，整体成本低，具有很强的实用性，现在全球最知名的三线阵相机是Leica公司生产的ADS系列相机。经过国内相关技术领域的科研院所多年的努力，成功研制出国产三线阵相机，不仅具有更高分辨率，还具有更快的数据获取效率，10分钟内可完成180平方公里地面数据的获取。
[0003] 三线阵相机在焦平面上安置三条CCD探测器，在飞行期间，三条CCD线阵同时成像，前视线阵(F)向前倾斜成像，下视线阵(N)垂直对地成像，后视线阵(B)向后倾斜成像，工作原理如图1所示。其中左图中S为摄影中心，Fs、Ns和Bs分别表示前视线阵、下视线阵和后视线阵对应的地面成像区域；右图中(xf,yf),(xn,yn)和(xb,yb)分别为同一地面点P(X,Y,Z)分别在T1时刻成像于前视线阵上的像点坐标，在T2时刻成像于下视线阵上的像点坐标和在T3时刻成像于后视线阵上的像点坐标。
[0004] 随着载荷平台向前推扫，相机以一定的频率连续对地面扫描成像，可在每个曝光时刻同时获得三条地面线状影像，通过连续飞行成像就可以获取地貌的整体影像，如图2所示。
[0005] 在对地定位处理上，三线阵相机与传统成像系统存在较大差异，其最大特点是连续推扫成像，影像行内连续，行间独立。为恢复影像行关系，三线阵相机安装有由GPS和IMU组成的POS系统，记录每个推扫行成像时刻的位置和姿态参数。POS记录可恢复影像，但无法满足专业测绘对地定位要求，为此还需结合影像进行域网平差模型解算。传统三线阵相机影像区域网平差模型包括低阶多项式模型、分段多项式模型和定向片内插模型，这些数学模型比较复杂，解算不仅需要大量地面控制点，而且还要求使用大内存、高性能计算机进行长时间的处理，完全无法适应国产三线阵高效数据获取，极大地限制了国产三线阵相机的发展。
[0006] 针对现有三线阵相机处理需要大量控制点，计算模型复杂耗时问题，本发明受卡尔曼滤波可实现连续信号稳定求解思想启发，推导了影像与POS结合的卡尔曼平差模型，并指出模型的具体使用步骤。采用本发明不仅对计算机内存性能无特别要求，而且可实现稀少控制点情况下高精度定位，有效提升国产三线阵相机的国际竞争力。
发明内容
[0007] 本发明针对现有三线阵相机处理需要大量控制点，计算模型复杂耗时问题，提供一种国产三线阵相机影像高精度对地定位处理方法，称为基于卡尔曼滤波模型的三线阵相机POS辅助对地定位法。
[0008] 本发明提供一种基于卡尔曼滤波模型的三线阵影像POS辅助对地定位方法，包括如下步骤：
[0009] 步骤1，建立局部坐标系作为统一的基准坐标系，将导航数据信息转换到基准坐标系中；
[0010] 步骤2，根据POS记录的位置姿态参数和相机参数将飞行获取的所用影像纠正到平均地面高的平面，形成中间影像，记为D0影像；
[0011] 步骤3，使用步骤2生产的D0影像开展影像匹配，提取影像连接点；
[0012] 步骤4，根据外业测量获取的地面控制点信息，在D0影像中，指定控制点影像位置；
[0013] 步骤5，建立卡尔曼平差模型，联合建立三线阵POS记录数据和影像扫描行之间的关系，根据POS系统获取的参数，建立K状态下的运动方程；所述POS系统获取的参数为航拍状态矢量，包括相机位置矢量、速度矢量和加速度矢量，以及相机姿态矢量、相应变化率矢量和加速度变化率矢量；
[0014] 步骤6，基于三线阵相机连续获取影像的过程符合动力学原理，根据运动方程，利用K状态下参数进行预估，计算K+1状态下状态参数的先验预测值X‑(K+1)；
[0015] 步骤7，根据运动方程，计算K+1时刻的先验预测值X‑(K+1)的误差协方差矩阵P‑(K+1)；
[0016] 步骤8，利用测量方程，建立K+1状态下先验预测值X‑(K+1)和测量值Z(K+1)之间的函数关系，并获取测量矩阵H(K+1)；
[0017] 步骤9，根据运动方程中获得的当前状态误差协方差矩阵和测量矩阵计算当前状态的卡尔曼增益Kg(K+1)；
[0018] 步骤10，将增益Kg(K+1)带入到更新方程中，对步骤6和步骤7所得结果进行修正，获取第K+1状态下状态矢量最佳后验估计值X(K+1)和控制点测量值Z(K+1)，同时计算第K+1状态下矢量的误差协方差矩阵P(K+1)；
[0019] 步骤11，判断终止条件，若不满足转到步骤6继续迭代，否则迭代结束，将误差校正后的位置姿态数据输出作为最终结果。
[0020] 而 且 ，步 骤 5 中 P O S 系 统 获 取 的 参 数 ，为 航 拍 状 态 矢 量包括相机位置矢量[Xs，Ys，Zs]、速度矢量[Xs'，Ys'，Zs']和加速度矢量[Xs”，Ys”，Zs”]，相机姿态(角度)矢量 相应变化率矢量 和加速度变化率矢量
[0021] 设卡尔曼运动方程为：
[0022]
[0023] 式中， 是指由K状态下状态参数的后验估计值X(K)计算得到的K+1状态下状态参数的理论真实值；W(K+1)是白噪声，G(K+1)是噪声系数；
[0024] A(K+1)是指由K状态到K+1状态的转移矩阵，其形式为A(K+1)＝diag[a,a,a,a,a,a]，diag[]表示分块对角阵，其中a为3×3的上三角阵，形式为：
[0025]
[0026] 其中，t表示由K状态到K+1状态之间的时间间隔，数值从POS记录数据中读取。
[0027] 而且，步骤6中，K+1状态下状态矢量的先验预测值X‑(K+1)预测值计算公式为，[0028] X‑(K+1)＝A(K+1)×X(K)+(G(K+1)×W(K+1))
[0029] 式中，X‑(K+1)是指由K状态下状态参数的后验估计值预测得到的K+1状态下状态参数的先验预测值。
[0030] 而且，步骤7中，K+1时刻的先验预测值X‑(K+1)的误差协方差矩阵P‑(K+1)，是X‑(K+1)与之差的协方差矩阵，由 的计算公式和X‑(K+1)的计算公式根据协方差矩阵计算方式得到如下形式，计算K+1时刻的误差协方差矩阵，计算公式为，
[0031] P‑(K+1)＝A(K+1)×P(K)×A(K+1)T+Q(K+1)
[0032] 式中，P‑(K+1)是X‑(K+1)对应的误差协方差矩阵；P(K)是K状态下X(K)对应的误差协方差矩阵，采用卡尔曼滤波的结果；Q(K)是噪声的协方差矩阵。
[0033] 而且，步骤8中，利用测量方程，建立K+1状态下先验预测值X‑(K+1)与控制点坐标之间的函数关系，所述测量方程为：
[0034] Z(K+1)＝h(X‑(K+1))+V(K+1)
[0035] 式中，Z(K+1)是K+1状态下引入的控制点地面坐标测量值，具体形式为：
[0036] Z(K+1)＝[x1K+1，y1K+1，x2K+1，y2K+1，x3K+1，y3K+1，…xnK+1，ynK+1]T，n为控制点总数，xiK+1和K+1yi 表示K+1状态下引入的第i个控制点的像片坐标(i∈[1,n])；
[0037] V(K+1)为测量噪声，服从预设的高斯分布；
[0038] h(X‑(K+1))为测量方程函数矩阵，h(X‑(K+1))＝[h1(X‑(K+1)),h2(X‑(K+1)),…,hn‑ T ‑(X(K+1))]，hi(X(K+1))是个非线性函数，采用摄影测量构像方程。
[0039] 而且，步骤9中，卡尔曼增益Kg(K+1)的计算公式如下：
[0040] Kg(K+1)＝P‑(K+1)×H(K+1)T/(H(K+1)×P‑(K+1)×H(K+1)T+R(K+1))
[0041] 式中，
[0042] Kg(K+1)为K+1状态下的卡尔曼增益；
[0043] P‑(K+1)是X‑(K+1)对应的误差协方差矩阵；
[0044] H(K+1)为线性化的测量矩阵，H(K+1)T是对应的转置矩阵；
[0045] R(K+1)是白噪声矩阵。
[0046] 而且，步骤10中，更新方程所用的计算公式为，
[0047] X(K+1)＝X‑(K+1)+Kg(K+1)×(Z(K+1)‑(H(K+1)×X‑(K+1)+V(K+1)))
[0048] P(K+1)＝P‑(K+1)–Kg(K+1)×H(K+1)×P‑(K+1)
[0049] 式中，
[0050] X(K+1)是该状态下参数的最佳后验估计值，是X‑(K+1)的新值；
[0051] Z(K+1)是该状态下引入的控制点坐标测量值；
[0052] P(K+1)是X(K+1)对应状态下参数的误差协方差矩阵，是P‑(K+1)的新值。
[0053] 本发明可以更快速、更准确地求解影像的定位参数，不仅计算实现简便，运算速率快，而且在误差模型具体形式不确定的情况下，仍然具有适用性。采用本方法可实现稀少控制点情况下的高精度定位，有效填补国产三线阵相机高精度数据处理的空白。与现有技术相比，本发明具有如下优点：
[0054] (1)数据模型简单严密，方程解算对计算机内存、性能无特别要求，因此自动定位流程实现方便，成本低廉。
[0055] (2)稳定，收敛性好，在误差源和误差参数形式不确定的情况下，仍然具有适用性，可以更快速、更可靠地求解影像的定位参数。
[0056] (3)受控制点数量影响小，在控制点比较少的情况下，仍然可实现整体高精度定位。
附图说明
[0057] 图1为本发明实施例国产三线阵相机成像原理示意图。
[0058] 图2为本发明实施例推扫过程和获取的影像示意图。
[0059] 图3为本发明实施例的飞机位置姿态参数定义示意图。
[0060] 图4为本发明实施例的处理过程流程图。
[0061] 图5为本发明实施例的卡尔曼滤波核心处理过程示意图。
具体实施方式
[0062] 下面结合附图和实施例对本发明的技术方案作进一步说明。
[0063] 本发明针对现有三线阵相机处理需要大量控制点，计算模型复杂耗时问题，提供一种国产三线阵相机影像高精度对地定位处理方法，实现国产三线阵相机影像高精度对地定位。其核心发明包括两个方面：一是结合卡尔曼滤波思想，设立影像外方位元素及其一阶导数、二阶导数作为待解方程未知数的全新平差模型。二是将影像点与影像内、外方位元素组成的构像方程作为协方差求解的观测方程，并指出方程的具体表述形式。
[0064] 卡尔曼滤波本身是一种递推线性最小方差估计算法，基本思想是以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型，在不同状态变化过程中，利用前一状态的后验估计值和当前状态的观测值来更新状态变量的估计，求出当前状态的后验估计值。卡尔曼滤波算法算法不仅计算简便，运算速率快，而且在误差模型具体形式不确定的情况下，仍然具有适用性。
[0065] 本发明经多方比较研究后注意到：三线阵相机推扫成像模式符合动力学模型，与卡尔曼滤波思想一致，可以建立影像与POS结合的卡尔曼平差模型。三线阵影像需要求解的参数包括影像成像时刻的影像位置[Xs，Ys，Zs]和影像姿态 从运动学角度出发可知，位置的变化就是速度，速度的变化是加速度，其数学描述就是一阶导数和二阶导数。
因此可以将三线阵影像定位求解参数设为
其
包含影像位置参数[Xs，Ys，Zs]、位置一阶导数速度参数[Xs'，Ys'，Zs']和位置二阶导数加速度参数[Xs”，Ys”，Zs”]，影像姿态参数 姿态一阶导数角速度参数
和姿态二阶导数角加速度参数 然后，根据卡尔曼滤波基本原理，联合建立三
线阵POS记录数据和影像扫描行之间的关系，建立运动方程，其中需要指出的是，运动方程的协方差观测求解需要用影像坐标与影像内、外方位元素组成的构像方程进行。
[0066] 如图4所示，本发明实施例的基于卡尔曼滤波模型的三线阵相机POS辅助对地定位法，流程包括以下步骤：
[0067] 步骤1，建立局部坐标系作为统一的基准坐标系，将导航数据信息转换到基准坐标系中；
[0068] 步骤2，根据POS记录的位置姿态参数和相机参数将飞行获取的原始影像纠正到平均地面高的平面，形成中间影像(实施例称为D0影像)。
[0069] 步骤3，使用步骤2生产的D0影像开展影像匹配，提取影像连接点。
[0070] 步骤4，根据外业测量获取的地面控制点信息，在D0影像中，指定控制点影像位置。具体实施时，也可由用户指定控制点影像位置，以加快处理速度。
[0071] 步骤5，建立卡尔曼平差模型。根据POS系统获取的参数(包括摄影测量求解的影像外方 位 元素 和 相机 安 装的 PO S 记录 数 据) ，可设 立 代求 解 未知 数 为作
为卡尔曼滤波参数，建立K状态下的运动方程(初始值使用飞机记录的第1行影像的POS值)。
ω， κ分别表示侧滚角、俯仰角，偏航角，其示意图见图3，其中侧滚角ω表示机体坐标系zb轴(机底方向)与通过机体xb轴(机头方向)的铅垂面间的夹角，俯仰角 表示机体xb轴与水平面的夹角，偏航角κ则表示机体xb轴的水平投影与正北方向的夹角。
[0072] 该矢量为18×1的列向量，包含影像位置参数[Xs，Ys，Zs]、影像位置一阶导数[Xs'，Ys'，Zs']和影像位置二阶导数[Xs”，Ys”，Zs”]，影像姿态参数 影像姿态一
阶导数 和影像姿态二阶导数
[0073] 设卡尔曼运动方程为：
[0074]
[0075] 式中， 是指由K状态下状态参数的后验估计值X(K)(卡尔曼滤波校正后的值)计算得到的K+1状态下状态参数的理论真实值，但是由于存在不确定性因素(模型误差、观测误差等)，该理论真实值无法直接计算得到，一般也用卡尔曼滤波校正后的后验估计值X(K+1)代替；W(K+1)是白噪声，直接按POS仪器的参数给定；G(K+1)是噪声系数，可以取单位阵。
[0076] A(K+1)是指由K状态到K+1状态的转移矩阵，其形式为A(K+1)＝diag[a,a,a,a,a,a]，diag[]表示分块对角阵，实际大小为18×18，其中a为3×3的上三角阵，其形式为：
[0077]
[0078] 其中，t表示由K状态到K+1状态之间的时间间隔，具体数值从POS记录数据中读取。
[0079] 步骤6，三线阵相机连续获取影像的过程符合动力学原理，根据卡尔曼运动方程，利用K状态下参数进行预估，计算K+1状态下状态参数的先验预测值X‑(K+1)。具体实施时，K的取值根据当前处理对象而定。
[0080] 实施例中，利用K状态下外方位参数及对应的误差协方差矩阵进行预估，计算K+1状态下状态参数的先验预测值及对应的误差协方差矩阵，其中K状态表示处理的第K张影像曝光时刻的状态，由于一张影像只对应一个POS值，也可称为第K点，先验预测值计算公式为：
[0081] X‑(K+1)＝A(K+1)×X(K)+(G(K+1)×W(K+1))
[0082] 式中，X‑(K+1)是指由K状态下状态参数的后验估计值预测得到的K+1状态下状态参数的先验预测值；A(K+1)是指由K状态到K+1状态的转移矩阵；X(K)是指K状态下状态参数的后验估计值，也就是上次的状态参数。
[0083] 具体实施时，步骤5中是理论上的方程，步骤6中是实际计算所用的方程，噪声项(G(K+1)×W(K+1))的具体值是未知的，只知道其高斯分布。
[0084] 也即，K+1状态下状态参数的理论真实值 或后验估计值X(K+1)无法直接计算得‑
到，不过可以先通过以下公式计算其先验预测值X(K+1)
[0085] X‑(K+1)＝A(K+1)×X(K)+(G(K+1)×W(K+1))
[0086] 其中噪声项G(K+1)×W(K+1)一般均值为0可不参与X‑(K+1)的计算，但是要参与其误差协方差矩阵的计算。
[0087] 步骤7，根据运动方程，计算K+1时刻的先验预测值X‑(K+1)的误差协方差矩阵P‑(K+1)。‑ ‑ ‑
[0088] 实施例中，K+1时刻的先验预测值X (K+1)的误差协方差矩阵P (K+1)，是X (K+1)与‑之差的协方差矩阵，可由 的计算公式和X(K+1)的计算公式根据协方差矩阵计算方式得
到如下形式，即计算K+1时刻的误差协方差矩阵，计算公式为：
[0089] P‑(K+1)＝A(K+1)×P(K)×A(K+1)T+Q(K+1)
[0090] 式中，P‑(K+1)是X‑(K+1)对应的误差协方差矩阵；P(K)是K状态下X(K)对应的误差协方差矩阵，也就是上一次卡尔曼滤波的结果；Q(K)是噪声的协方差矩阵。
[0091] 步骤8，利用测量方程，建立K+1状态下先验预测值X‑(K+1)和测量值Z(K+1)(也就是控制点坐标)的函数关系，并获取测量矩阵H(K+1)。
[0092] 实施例中，利用测量方程，建立K+1状态下先验预测值X‑(K+1)与控制点坐标之间的函数关系，所述测量方程为：
[0093] Z(K+1)＝h(X‑(K+1))+V(K+1)
[0094] 式中，Z(K+1)是K+1状态下引入的控制点地面坐标测量值，具体形式为：
[0095] Z(K+1)＝[x1K+1，y1K+1，x2K+1，y2K+1，x3K+1，y3K+1，…xnK+1，ynK+1]T，n为控制点(包括连接点)K+1 K+1总数，xi 和yi 表示K+1状态下引入的第i个控制点的像片坐标(i∈[1,n])；
[0096] V(K+1)为测量噪声，服从均值和方差分别为r和R的高斯分布，具体实施时均值r和方差R可预先设定取值，一般情况下均值为0，方差由测量仪器精度给定；
[0097] h(X‑(K+1))为测量方程函数矩阵，h(X‑(K+1))＝[h1(X‑(K+1)),h2(X‑(K+1)),…,hn‑ T ‑(X(K+1))]，hi(X(K+1))是个非线性函数，这里就是摄影测量构像方程，具体计算公式为：
[0098]
[0099]
[0100] 其中，x，y是控制点的投影坐标(投影到影像上的坐标)，Xs，Ys，Zs是就是影像位置参数[Xs，Ys，Zs]，XA，YA，ZA则是控制点的物方空间坐标，而a1，a2，a3，b1，b2，b3，c2，c3，c3是影像姿态参数 对应的旋转矩阵，其计算公式为：
[0101]
[0102] 步骤9，根据运动方程中获得的当前状态误差协方差矩阵和测量矩阵计算当前状态的卡尔曼增益Kg(K+1)，计算公式为：
[0103] Kg(K+1)＝P‑(K+1)×H(K+1)T/(H(K+1)×P‑(K+1)×H(K+1)T+R(K+1))
[0104] 式中，
[0105] Kg(K+1)为K+1状态下的卡尔曼增益；
[0106] P‑(K+1)是X‑(K+1)对应的误差协方差矩阵；
[0107] H(K+1)为线性化的测量矩阵，H(K+1)T是对应的转置矩阵；
[0108] R(K+1)是白噪声矩阵，这里用直接由POS设备给定的参数按高斯分布产生；
[0109] 步骤10，将增益Kg(K+1)带入到更新方程中，对步骤6和步骤7的预估值进行修正，获取第K+1状态下状态矢量最佳后验估计值X(K+1)和控制点测量值Z(K+1)，同时计算第K+1状态下矢量的误差协方差矩阵P(K+1)，计算公式分别为：
[0110] X(K+1)＝X‑(K+1)+Kg(K+1)×(Z(K+1)‑(H(K+1)×X‑(K+1)+V(K+1)))
[0111] P(K+1)＝P‑(K+1)–Kg(K+1)×H(K+1)×P‑(K+1)
[0112] 式中，
[0113] X(K+1)是该状态下参数的最佳后验估计值，是X‑(K+1)的新值；
[0114] Z(K+1)是该状态下引入的控制点坐标测量值；
[0115] P(K+1)是X(K+1)对应状态下参数的误差协方差矩阵，是P‑(K+1)的新值。
[0116] 步骤11，参见图5，判断终止条件是否为最后一个点，若不满足转到步骤6继续迭代，利用K+1状态的新值计算下一个点，即K+2点，具体实施时可以令K＝K+1，然后返回步骤6进行同样的操作，若是则迭代结束，将误差校正后的位置姿态数据输出作为最终结果。
[0117] 具体实施时，本发明技术方案提出的方法可由本领域技术人员采用计算机软件技术实现自动运行流程，实现方法的系统装置例如存储本发明技术方案相应计算机程序的计算机可读存储介质以及包括运行相应计算机程序的计算机设备，也应当在本发明的保护范围内。
[0118] 在一些可能的实施例中，提供一种基于卡尔曼滤波模型的三线阵影像POS辅助对地定位系统，包括处理器和存储器，存储器用于存储程序指令，处理器用于调用存储器中的存储指令执行如上所述的一种基于卡尔曼滤波模型的三线阵影像POS辅助对地定位方法。
[0119] 在一些可能的实施例中，提供一种基于卡尔曼滤波模型的三线阵影像POS辅助对地定位系统，包括可读存储介质，所述可读存储介质上存储有计算机程序，所述计算机程序执行时，实现如上所述的一种基于卡尔曼滤波模型的三线阵影像POS辅助对地定位方法。
[0120] 本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。
