CN104764452A - 一种基于惯性和光学跟踪系统的混合位姿跟踪方法 - Google Patents

Abstract

本发明公开了一种基于惯性和光学跟踪系统的混合位姿跟踪方法，通过将惯性跟踪系统和光学跟踪系统进行结合使用，可以解决在标志物被部分遮挡时对目标的准确位置和姿态跟踪；利用光学跟踪结果修正惯性传感器数据，消除了偏置误差，提高了惯性跟踪模块在进行姿态估计时的精度；在利用卡尔曼滤波器与惯性跟踪系统进行姿态跟踪时，通过建立加速度误差模型，在测量噪声协方差矩阵中引入突变速运动测量噪声协方差，当系统发生线性运动时，状态方程的测量更新方程中，对测量值的置信程度也会降低，使得状态矢量中估计值占有更大的置信度，减小了加速度计测量值因突变线性加速度的影响而导致的对状态矢量的影响。

Description

一种基于惯性和光学跟踪系统的混合位姿跟踪方法

技术领域

本发明涉及智能传感与人机交互领域，尤其涉及一种基于惯性和光学跟踪系统的混合位姿跟踪方法。

背景技术

由于惯性跟踪系统根据陀螺仪的角速度、加速度计的加速度以及磁传感器的磁强度计算目标的姿态角，具有刷新频率较高以及测量延迟较低的优点且跟踪时不依赖于外部标志点。将光学与惯性跟踪系统进行融合能够获得实时性更好、精度更高的跟踪结果，在出现遮挡问题时可以在较短时间继续进行位置和姿态跟踪。

国内外研究者对惯性-光学混合跟踪方法进行了广泛研究。Young等人于2006年提出了混合式跟踪系统，其中包含了一个由6台摄像机构成的Vicon光学跟踪系统和一个固定在目标上的装配了反光标志点的惯性跟踪模块(InertialMeasurement Unit,IMU)，在保持光学跟踪系统的高精度的同时将刷新频率提高到100Hz(参见Young M C,Young S S,Sang K.Pose estimation fromlandmark-based vision and inertial sensors.In:Proceedings of the 2006InternationalJoint Conference on SICE-ICASE.Bexco,Busan,Korea:IEEE,2006,1668-1671.)。N.Parnian于2008年提出了用于跟踪笔状目标的混合跟踪系统，安装了IMU的目标的尖部可以被四个依弧线排列的摄像机所识别，混合跟踪系统提高了光学跟踪系统的刷新频率和位置跟踪精度(参见Maidi M,Ababsa F,Mallem M.Vision-inertial tracking system for robust fiducials registration in augmented reality.In:Proceedings of the 2009IEEE Symposium on Computational Intelligence forMultimedia Signal and Vision Processing.Nashville,TN,USA:IEEE,2009.83-90)。Maidi于2009年提出了一个由两个摄像机和三个标志物组成的光学跟踪系统(刷新率为55Hz)以及一个跟标志物绑定的惯性跟踪模块(刷新率为500Hz)组成的混合跟踪系统，实验结果表明该系统可以实现在短时间光学遮挡条件下的姿态跟踪(参见Waechter C,Huber M,Keitler P,Schlegel M,Pustka D,Klinker G.Amulti-sensor platform for wide-area tracking.In:Proceedings of the 9th IEEEInternational Symposium on Mixed and Augmented Reality.Seoul,Korea:IEEE,2010.275-276.)。上述各方法通过融合惯性和光学跟踪方法有效提高了跟踪系统的刷新率并实现了在出现遮挡时进行短时间继续跟踪，但是在跟踪目标位置时会产生随时间累计的漂移误差且其自身会产生偏置误差、线性加速度误差等，上述方法并不能够实现在长时间遮挡情况下的准确跟踪。

发明内容

有鉴于此，本发明提供了一种基于惯性和光学跟踪系统的混合位姿跟踪方法，可以解决在标志点被部分遮挡时对目标的准确位置和姿态跟踪，同时利用光学跟踪结果修正惯性传感器数据，消除了偏置误差。

为了解决上述技术问题，本发明是这样实现的：

本发明的一种基于惯性和光学跟踪系统的混合位姿跟踪方法，包括：

1)、在光学跟踪系统中的标志物上的标志点未被遮挡时，利用光学跟踪系统捕获当前时刻目标的位置和姿态信息，作为位姿跟踪的结果；

2)、在所述标志物上的部分标志点被遮挡，光学跟踪系统只能获得标志点的位置信息但无法获得目标的姿态信息时，执行如下步骤：

S21、当前时刻，采用光学跟踪系统得到所述标志点的三维空间坐标X_n、Y_n和Z_n；

S22、采用加速度计、磁传感器和陀螺仪组成的惯性跟踪系统获得所述标志物当前时刻的角速度、加速度和磁场的测量值，进而采用卡尔曼滤波器得到表征标志物姿态信息的俯仰角、横滚角和航向角，以及当前时刻标志物的姿态旋转矩阵R_n；

S23、确定标志物上原点与所述标志点的三维空间位移：ΔX_n、ΔY_n和ΔZ_n；

S24、基于如下公式得到当前时刻所述原点的三维空间坐标X₀、Y₀和Z₀：

$\left[\begin{array}{c}{X}_0\\ Y_0\\ Z_0\end{array}\right]=\left[\begin{array}{c}{X}_n\\ Y_n\\ Z_n\end{array}\right]-R_n\·\left[\begin{array}{c}\mathrm{\Δ}{X}_n\\ \mathrm{\Δ}{Y}_n\\ \mathrm{\Δ}{Z}_n\end{array}\right];$

S25、根据目标与所述原点既定的位置关系，得到目标的位置信息；

如此，将S22获得的当前时刻的姿态信息和S25获得的当前时刻目标的位置信息，作为对目标位姿跟踪的结果。

较佳的，在1)中，光学跟踪系统进行位姿跟踪的同时，采用陀螺仪、加速度计和磁传感器分别对目标的角速度、加速度和磁场进行测量；然后根据光学跟踪系统获得的姿态信息，使用余弦算法计算出目标当前位姿对应的理想的角速度、加速度和磁场，并分别与陀螺仪、加速度计以及磁传感器的测量值进行比对，获得陀螺仪、加速度计以及磁传感器的偏置误差；

在S22中，在采用惯性跟踪系统获得标志物的角速度、加速度和磁场的测量值后，采用1)中的偏置误差分别对该三个测量值进行补偿，再根据补偿后的角速度、加速度和磁场，得到S22中当前时刻目标的姿态信息以及标志物的姿态旋转矩阵R_n。

较佳的，所述S22中，采用卡尔曼滤波器获得当前时刻目标的姿态信息的方法为：

S221、在时间更新方程中对目标的状态矢量进行估计：

其中，A为所述陀螺仪测得的目标在三个敏感轴上角速度测量值构成的滤波器增益矩阵；为当前时刻目标状态矢量估计值，为卡尔曼滤波器上一时刻输出的目标状态矢量；

S222、对卡尔曼滤波器的传递函数的估计值进行更新：

$P_k^{-}=A{P}_{k-1}{A}^T+Q$

其中，Q值为系统噪声协方差矩阵；A^T为增益矩阵A的转置矩阵；P_k-1为卡尔曼滤波器上一时刻输出的传递函数；

S223、通过卡尔曼滤波器对目标姿态信息进行测量更新，具体为：

基于步骤S221中更新得到的传递函数的估计值获得卡尔曼滤波器的测量置信参数K_k：

$K_k=P_k^{-}{H}^T{(H{P}_k^{-}{H}^T+R_k)}^{-1}$

其中，H为测量模型与估计模型间转换矩阵：

[q₁ q₂ q₃ q₄]^T表示组成目标的状态矢量估计值的旋转四元数，即R_k＝R_a+R_b为测量噪声协方差矩阵，由匀速运动噪声协方差R_a和突变速运动测量噪声协方差R_b组成，R_b＝ACC_x ²+ACC_y ²+ACC_z ²-g²，其中ACC_x、ACC_y和ACC_z分别为加速度传感器测得的目标在三个敏感轴上的加速度分量，g为重力加速度；

S224、对目标的状态矢量的测量结果进行更新：

$Z_k=\left[\begin{array}{c}\mathrm{\θ}\\ \mathrm{\γ}\\ \mathrm{\ψ}\end{array}\right]$ 表示目标的姿态，其中，θ＝arcsin(ACC_x)表示目标的俯仰角，表示目标的横滚角，表示目标的航向角，A＝MAG_xcos(θ)+MAG_ysin(θ)+MAG_zcos(θ)sin(γ)，B＝MAG_ycos(γ)+MAG_zsin(γ)，MAG_x、MAG_y和MAG_z分别表示磁场传感器获得的目标所在位置的地球磁场在体坐标系的三个敏感轴上的分量；

S225、对传递函数的测量结果P_k进行更新：

$P_k=(1-K_{k}H)P_k^{-}$

S225、从所述目标状态矢量的测量结果中提取当前时刻的目标姿态信息；将当前时刻测量结果和传递函数的测量结果P_k输出并作为下一时刻滤波器的输入结果，返回S221，进行下一时刻的目标姿态跟踪。

较佳的，所述步骤1)中，当有两个标志点未被遮挡时，采用S21-S24的方法分别基于各标志点的位置信息得到所述原点的位置信息并求均值，在S25中，利用该原点位置信息的均值，得到目标的位置。

本发明具有如下有益效果：

(1)本发明提出的混合位姿跟踪方法，通过将惯性跟踪系统和光学跟踪系统进行结合使用，可以解决在标志物被部分遮挡时对目标的准确位置和姿态跟踪；

(2)本发明利用光学跟踪结果修正惯性传感器数据，消除了偏置误差，提高了惯性跟踪模块在进行姿态估计时的精度；

(3)本发明在利用卡尔曼滤波器与惯性跟踪系统进行姿态跟踪时，通过建立加速度误差模型，在测量噪声协方差矩阵中引入突变速运动测量噪声协方差，当系统发生线性运动时，状态方程的测量更新方程中，对测量值的置信程度也会降低，使得状态矢量中估计值占有更大的置信度，减小了加速度计测量值因突变线性加速度的影响而导致的对状态矢量的影响。

附图说明

图1是本发明的偏置误差校正算法。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

本发明以手术中对手术工具进行长时间位姿跟踪为例进行说明。手术过程中，需要对手术工具相对于病灶部分的位置和姿态进行实时的精确计算，以便引导手术的顺利进行。光学跟踪系统通过手术工具上设置的标志物对手术工具进行定位以及姿态跟踪。但在手术过程中，经常出现标志物被遮挡的情况，当标志物上只有两个或一个标志点未被遮挡，则会导致光学跟踪系统只能利用标志物获得位置信息，却无法获得姿态信息，因此，本发明提出一种基于惯性和光学跟踪系统的混合位姿跟踪方法，可以解决在标志点被部分遮挡时对标志物的准确位置和姿态跟踪，具体方法如下：

1)、正常情况下，作为位姿跟踪的标志物上的标志点未被遮挡，此时采用光学跟踪系统得到当前时刻标志物的位置和姿态信息，作为位姿跟踪的结果。

2)、当出现标志物上的标志点被遮挡，即光学跟踪系统只能观察一个或两个标志点时，光学跟踪系统只能获得标志物的位置信息，由于三个标志点才能确定标志物的姿态，因此，一个或两个标志点的信息不足以获得标志物的姿态信息，则执行如下步骤：

S21、当前时刻，采用光学跟踪系统得到未被遮挡的标志点的三维空间坐标X_n、Y_n和Z_n；

S22、采用加速度计、磁传感器和陀螺仪组成的惯性跟踪系统获得所述标志物当前时刻的角速度、加速度和磁场的测量值，采用卡尔曼滤波器获得当前时刻标志物的俯仰角θ、横滚角γ和航向角ψ，最终得到当前时刻标志物的姿态旋转矩阵：

$R_n=\left[\begin{array}{ccc}\cos \left(\mathrm{\ψ}\right)\cos \left(\mathrm{\θ}\right)& \cos \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\θ}\right)\cos \left(\mathrm{\γ}\right)& \cos \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\θ}\right)\sin \left(\mathrm{\γ}\right)-\sin \left(\mathrm{\ψ}\right)\cos \left(\mathrm{\γ}\right)\\ -\sin \left(\mathrm{\θ}\right)& \cos \left(\mathrm{\θ}\right)\cos \left(\mathrm{\γ}\right)& \cos \left(\mathrm{\θ}\right)\sin \left(\mathrm{\γ}\right)\\ \sin \left(\mathrm{\ψ}\right)\cos \left(\mathrm{\θ}\right)& \sin \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\θ}\right)\cos \left(\mathrm{\γ}\right)-\cos \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\γ}\right)& \sin \left(\mathrm{\ψ}\right)\sin \left(\mathrm{\θ}\right)\sin \left(\mathrm{\γ}\right)+\cos \left(\mathrm{\ψ}\right)\cos \left(\mathrm{\γ}\right)\end{array}\right];$

S23、确定标志物上原点与所述标志点的三维空间位移：ΔX_n、ΔY_n和ΔZ_n；

S24、上述步骤已经确定了标志物的旋转矩阵以及原点与所述标志点的三维空间位移，则旋转矩阵与三维位移的乘积即为原点在全局坐标系中的空间位移，再用标志点坐标减去该全局坐标系中的空间位移，则得到原点在全局坐标系中的坐标X₀、Y₀和Z₀：

$\left[\begin{array}{c}{X}_0\\ Y_0\\ Z_0\end{array}\right]=\left[\begin{array}{c}{X}_n\\ Y_n\\ Z_n\end{array}\right]-R_n\·\left[\begin{array}{c}\mathrm{\Δ}{X}_n\\ \mathrm{\Δ}{Y}_n\\ \mathrm{\Δ}{Z}_n\end{array}\right];$

S25、根据可见标志点与所述标志物原点既定的位置关系，得到标志物的位置信息；

需要说明的是，当有两个标志点未被遮挡时，采用上述S21-S24的方法分别基于各标志点的位置信息得到所述原点的位置信息并求均值，本步骤中，利用该原点位置信息的均值，得到标志物的位置；

标志物如此，将S25获得的当前时刻标志物的位置信息和S22获得的当前时刻的姿态信息，作为位姿跟踪的结果。

在1)中，光学跟踪系统进行位姿跟踪的同时，采用陀螺仪、加速度计和磁传感器分别对标志物的角速度、加速度和磁场进行测量；然后根据光学跟踪系统获得的姿态信息，使用余弦算法计算出标志物当前位姿对应的角速度、加速度和磁场，由于光学跟踪系统的精确度更高，因此由光学跟踪系统的姿态信息解算的角速度、加速度和磁场值较惯性系统的测量值具有更高的精确度，因此，可用，将光学跟踪系统的姿态信息解算的角速度、加速度和磁场值分别与陀螺仪、加速度计以及磁传感器的测量值进行比对，获得陀螺仪、加速度计以及磁传感器的偏置误差；

在S22中，在采用惯性跟踪系统获得标志物的的角速度、加速度和磁场的测量值后，采用1)中的偏置误差分别对该三个测量值进行补偿，再根据补偿后的角速度、加速度和磁场，分别得到S22中标志物的姿态信息和旋转矩阵R_n标志物，如此，在光学跟踪系统的姿态跟踪失效不得已采用惯性跟踪系统时，可提高惯性跟踪系统的跟踪精度。

考虑到加速度计测量重力加速度值时，可能会因突变的线性加速度的出现而受到影响，本发明还对卡尔曼滤波过程进行了改进，具体为步骤S22中采用卡尔曼滤波器获得当前时刻标志物的姿态信息的方法：

S221、在时间更新方程中对标志物姿态状态矢量进行估计：

其中，A为所述陀螺仪测得的标志物在三个敏感轴上角速度测量值构成的滤波器增益矩阵；为当前时刻标志物姿态状态矢量估计值，为卡尔曼滤波器上一时刻输出的标志物状态矢量；

S222、对卡尔曼滤波器的传递函数的估计值进行更新：

$P_k^{-}=A{P}_{k-1}{A}^T+Q$

其中，Q值为系统噪声协方差矩阵；A^T为增益矩阵A的转置矩阵；P_k-1为卡尔曼滤波器上一时刻输出的传递函数；

S223、在卡尔曼滤波器中对标志物姿态信息进行测量更新，具体为：

基于步骤S221中更新得到的传递函数的估计值获得卡尔曼滤波器的测量置信参数K_k：

$K_k=P_k^{-}{H}^T{(H{P}_k^{-}{H}^T+R_k)}^{-1}$

其中，通过如下公式获得测量模型与估计模型间的转换矩阵H：

[q₁ q₂ q₃ q₄]^T表示组成目标的状态矢量估计值的旋转四元数，即R_k为测量噪声协方差矩阵。为了消除突变线性加速度对加速度计测量重力加速度值时产生的影响，本发明定义了加速度误差模型并在定义测量噪声协方差时，将R_k分为匀速运动噪声协方差R_a和突变速运动测量噪声协方差R_b：

R_k＝R_a+R_b

加速度计被用来测量地球重力场在三轴上的分量从而推导出标志物的当前姿态。在理想情况下，三轴上的加速度分量平方和应为重力加速度g的平方。然而，重力加速度计的测量值还和传感器的线性加速度大小有关，这部分与重力加速度无关的线性加速度被视为姿态测量中的误差，线性加速度误差模型通过如下方程进行定义：

R_b＝ACC_x ²+ACC_y ²+ACC_z ²-g²

其中ACC_x、ACC_y和ACC_z分别为加速度传感器测得的加速度在标志物三个敏感轴上的分量；

S224、对标志物姿态状态矢量的测量结果进行更新：

表示标志物的姿态，其中，θ＝arcsin(ACC_x)表示标志物的俯仰角，表示标志物的横滚角，表示标志物的航向角，A＝MAG_xcos(θ)+MAG_ysin(θ)+MAG_zcos(θ)sin(γ)，B＝MAG_ycos(γ)+MAG_zsin(γ)，MAG_x、MAG_y和MAG_z分别表示磁场传感器获得的标志物所在位置的地球磁场在体坐标系的三个敏感轴上的分量；利用加速度计和磁传感器进行姿态解算是通过离散测量和余弦算法实现的，其结果不会受到漂移误差的影响。

当系统发生线性运动时，加速度值的平方和将大于或小于g的平方，他们之间的差值构成了本方法定义的加速度误差模型。当此误差增加，R_b和R_k会随之增加进而导致K_k减小。于是在测量更新方程(1)中对测量值的置信程度也会降低，使得状态矢量中估计值占有更大的置信度，减小了加速度计测量值因突变线性加速度的影响而导致的对状态矢量的影响。

S225、对传递函数P_k进行更新：

$P_k=(1-K_{k}H)P_k^{-}$

S225、标志物状态矢量的测量结果即为当前时刻的标志物姿态跟踪结果；将当前时刻测量结果和传递函数的测量结果P_k输出并作为下一时刻滤波器的输入结果，返回S261，进行下一时刻的标志物姿态跟踪。

当全部标志点都可见时，光学跟踪方法能够同时获取标志板的6自由度位置和姿态信息。当任一标志点被遮挡时，纯光学跟踪系统只能得到可见标志点的空间位置但是不能计算标志物的标志板的三维姿态及旋转矩阵R。本发明在部分标志点被遮挡时通过惯性跟踪系统继续跟踪标志物的三维姿态以及旋转矩阵R，进而得到完整的位置和姿态跟踪结果解决了光学遮挡问题。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于惯性和光学跟踪系统的混合位姿跟踪方法，其特征在于，包括：

1)、在光学跟踪系统中的标志物上的标志点未被遮挡时，利用光学跟踪系统捕获当前时刻目标的位置和姿态信息，作为位姿跟踪的结果；

2)、在所述标志物上的部分标志点被遮挡，光学跟踪系统只能获得标志点的位置信息但无法获得目标的姿态信息时，执行如下步骤：

S21、当前时刻，采用光学跟踪系统得到所述标志点的三维空间坐标X_n、Y_n和Z_n；

S22、采用加速度计、磁传感器和陀螺仪组成的惯性跟踪系统获得所述标志物当前时刻的角速度、加速度和磁场的测量值，进而采用卡尔曼滤波器得到表征标志物姿态信息的俯仰角、横滚角和航向角，以及当前时刻标志物的姿态旋转矩阵R_n；

S23、确定标志物上原点与所述标志点的三维空间位移：ΔX_n、ΔY_n和ΔZ_n；

S24、基于如下公式得到当前时刻所述原点的三维空间坐标X₀、Y₀和Z₀：

$\left[\begin{array}{c}{X}_0\\ Y_0\\ Z_0\end{array}\right]=\left[\begin{array}{c}{X}_n\\ Y_n\\ Z_n\end{array}\right]-R_n\·\left[\begin{array}{c}{\mathrm{\ΔX}}_n\\ {\mathrm{\ΔY}}_n\\ {\mathrm{\ΔZ}}_n\end{array}\right];$

S25、根据目标与所述原点既定的位置关系，得到目标的位置信息；

如此，将S22获得的当前时刻的姿态信息和S25获得的当前时刻目标的位置信息，作为对目标位姿跟踪的结果。

2.如权利要求1所述的一种基于惯性和光学跟踪系统的混合位姿跟踪方法，其特征在于：

在1)中，光学跟踪系统进行位姿跟踪的同时，采用陀螺仪、加速度计和磁传感器分别对目标的角速度、加速度和磁场进行测量；然后根据光学跟踪系统获得的姿态信息，使用余弦算法计算出目标当前位姿对应的理想的角速度、加速度和磁场，并分别与陀螺仪、加速度计以及磁传感器的测量值进行比对，获得陀螺仪、加速度计以及磁传感器的偏置误差；

在S22中，在采用惯性跟踪系统获得标志物的角速度、加速度和磁场的测量值后，采用1)中的偏置误差分别对该三个测量值进行补偿，再根据补偿后的角速度、加速度和磁场，得到S22中当前时刻目标的姿态信息以及标志物的姿态旋转矩阵R_n。

3.如权利要求1或2所述的一种基于惯性和光学跟踪系统的混合位姿跟踪方法，其特征在于，所述S22中，采用卡尔曼滤波器获得当前时刻目标的姿态信息的方法为：

S221、在时间更新方程中对目标的状态矢量进行估计：

其中，A为所述陀螺仪测得的目标在三个敏感轴上角速度测量值构成的滤波器增益矩阵；为当前时刻目标状态矢量估计值，为卡尔曼滤波器上一时刻输出的目标状态矢量；

S222、对卡尔曼滤波器的传递函数的估计值进行更新：

$P_k^{-}={\mathrm{AP}}_{k-1}{A}^T+Q$

其中，Q值为系统噪声协方差矩阵；A^T为增益矩阵A的转置矩阵；P_k-1为卡尔曼滤波器上一时刻输出的传递函数；

S223、通过卡尔曼滤波器对目标姿态信息进行测量更新，具体为：

基于步骤S221中更新得到的传递函数的估计值P_k ^-，获得卡尔曼滤波器的测量置信参数K_k：

$K_k=P_k^{-}{H}^T{({\mathrm{HP}}_k^{-}{H}^T+r_k)}^{-1}$

其中，H为测量模型与估计模型间转换矩阵：

[q₁ q₂ q₃ q₄]^T表示组成目标的状态矢量估计值的旋转四元数，即R_k＝R_a+R_b为测量噪声协方差矩阵，由匀速运动噪声协方差R_a和突变速运动测量噪声协方差R_b组成，其中ACC_x、ACC_y和ACC_z分别为加速度传感器测得的目标在三个敏感轴上的加速度分量，g为重力加速度；

S224、对目标的状态矢量的测量结果进行更新

$Z_k=\left[\begin{array}{c}\mathrm{\θ}\\ \mathrm{\γ}\\ \mathrm{\ψ}\end{array}\right]$ 表示目标的姿态，其中，θ＝arcsin(ACC_x)表示目标的俯仰角，表示目标的横滚角，表示目标的航向角，A＝MAG_xcos(θ)+MAG_ysin(θ)+MAG_zcos(θ)sin(γ)，B＝MAG_ycos(γ)+MAG_zsin(γ)，MAG_x、MAG_y和MAG_z分别表示磁场传感器获得的目标所在位置的地球磁场在体坐标系的三个敏感轴上的分量；

S225、对传递函数的测量结果P_k进行更新：

$P_k=(1-K_{k}H)P_k^{-}$

S225、从所述目标状态矢量的测量结果中提取当前时刻的目标姿态信息；将当前时刻测量结果和传递函数的测量结果P_k输出并作为下一时刻滤波器的输入结果，返回S221，进行下一时刻的目标姿态跟踪。

4.如权利要求1或2所述的一种基于惯性和光学跟踪系统的混合位姿跟踪方法，其特征在于，所述步骤1)中，当有两个标志点未被遮挡时，采用S21-S24的方法分别基于各标志点的位置信息得到所述原点的位置信息并求均值，在S25中，利用该原点位置信息的均值，得到目标的位置。