进行序贯重要性采样时，若小于设定的阈值，则应当采用重采样的方法克 服粒子的权值退化，重采样方法舍弃权值较小的粒子，代之以权值较大的粒子。重采样 过程满足/<·^ = <)= iif条件下，将粒子集合
更新为
?:本发明 中当/^<0.5时，采用基于重要性采样的残差重采样更新粒子。残差重采样具有效率 高、实现方便的特点。设
其中|_」为取整操作。残余重采样采用新的权值
选择剩余下的
个粒子，残差重采样的主要过程为：
[0053] (1)计算剩余粒子的权值累计量％.，/= V
[0054] (2)生成;^个在[0, 1]区间内均匀分布的随机数。
[0055] (3)对于每个μ \寻找归一化权值累计量大于或者等于μ1的最小标号m，即 λ J μ 1Uni。当μ1落在区间[λ λ J时，<被复制一次。
[0056] 这样，每个粒子.tf经重采样后的个数为步骤（3)中所选中的若干粒子数目与Ν1。
[0057] 步骤五：对惯性导航系统的状态&进行估计
[0058] 贝叶斯估计利用概率分布的形式估计非线性系统的状态Xk。根据现有的观测量 Yk (Yk表示y i，y2，…，yk)，要估计出重力辅助惯导定位系统的位置信息Xk。依据贝叶斯公式 有：
[0060] 由式（19)可知，分母为一个与Xk无关的归一化常量，它可以保证P(X kIYk)的积分 为1。由式（20)可知测量误差％的概率密度函数p (yk I Xk)，即为先验概率密度函数：
[0061] P (yk I Xk) = / δ (yk-hk (xk)-Vk) p (vk) d Vk (29)
[0062] 其中δ⑴为狄拉克函数。
[0063] 由于观测值yk只与当前状态X k有关，则P (X k I Xk Yk i) = P (xk |xk i)，根据全概率 公式可知：
[0064] p (xk，xk i I Yk D = p (xk I xk i，Yk D p (xk i I Yk D (30)
[0065] 化简并进行积分可得CK方程：
[0066] p (xk I Yk !) = / P (xk I Xk !) P (xk ! I Yk !) d Xk ! (31)
[0067] 其中，p (xk ! I Yk D假设已知，p (xk I xk D为状态的Markov过程。
[0068] 即根据最小均方差（MMSE)准则，将条件均值作为系统状态Xk的最优估计值，即
[0069]
[0070] 则上式（28) (29) (31)组成了贝叶斯估计系统状态Xk的基本公式。式（29)为概 率密度函数，式（28)、（29)分别为位置更新和测量更新公式。由上述可知，贝叶斯估计需要 进行积分运算，非线性系统多采用近似算法解决积分问题，从而得到状态的最优估计。基于 贝叶斯估计的粒子滤波利用所求状态空间中的大量样本点来近似逼近待估变量的后验概 率分布，从而将积分问题转换为有限样本点的求和问题。
[0071] 粒子滤波就是寻找一组在状态空间中传播的随机样本对后验概率密度函数 P (xk I Yk)进行近似，以样本均值代替条件均值E [xk IYJ。将服从p (xk I Yk)分布的随机样本称 为粒子。
[0072] 设#为从后验概率密度函数P (xk I Yk)中获取的采样粒子，则对于要估计的惯导 位置Xk，条件均值E [xk IYJ可以用求和方式逼近，即
[0074] 此时进行步骤（三）中的序贯重要性采样，生成粒子，当采样粒子数目很大时，式 (13)便可逼近真实的后验概率密度。因此要估计的惯导位置Xk的条件均值E[xk|Y k]可表 示为：
[0076] 此时在进行步骤（四）重采样，要估计的惯导位置Xk的条件均值E [xk IYJ可表示 为：
[0078] 输出此时的惯导位置X1^估计值P k。
[0079] 步骤六：判断单点匹配结果是否可信
[0080] 单点匹配结果也应满足惯导相邻点之间位相关性，则单点匹配结果 {P' k4，P' k3，P' k2，P' kl，P' k}(k 彡 4)中的
也应 满足式（21)。以为例，若满足，则将P',的匹配结果视为可信点，若不满足，则在点 匹配过程中加入^的限定的条件得到新的重要性函数，根据@由式（21)所确 定的可信范围重新计算状态变量的转移概率密度函数，得到，进行步骤（五），得 到新的匹配结果|?。
[0081] 步骤七：计算矢量匹配结果权值
[0082] 若I
都不满足式（21)，则得到四个局部最优解
利用加权最小二乘估计得到最终估计值3加权最小二乘估 计观测模型为
[0083] βΡ+/ι (33)
[0084] 其中V表示采样点{Pk 4, Pk 3, Pk 2, Pk J惯导系统输出的速度IVk 4, Vk 3, Vk 2, Vk J，β 为系数矩阵[Pk 4, Pk3. Pk2, Pk In为观测噪声。取σ 1= Ivi-W1P1I,求加权偏差平方和 s最小的Wk，其中
[0086] 求得[wk 4, Wk 3, Wk 2, wk J，然后归一化权值，求得Pk的最终估计值
[0087] 步骤八：按照步骤（一）中的Λ Uk更新p (X k I Xk i)，再根据步骤（四）更新粒子权 值，进行k+Ι时刻的惯导位置估计，再进行步骤（六）（七），得到矢量匹配结果。
[0088] 为了说明本发明的效果，采用桑迪亚算法在相同的实验设置下进行试验，采用本 发明后匹配轨迹在匹配区域内对惯导轨迹起到了良好的校正作用，经炜度误差明显减小， 经过多次试验，本发明的经炜度误差平均值与传统桑迪亚算法比较的结果如表1所示。 [0089] 表1 :误差结果
CN 105180938 A 说明书 8/8 页
[0095] 其中若单点粒子滤波结果无须进行矢量匹配则视为可信。如表2所示，在重力异 常变化率大的区域，单点粒子滤波结果可信，在重力异常变化率相对小的区域，就要进行矢 量匹配。从仿真结果可以看出，在重力异常变化率较小的区域，单点粒子滤波的平均圆概率 误差增量平均值比在重力异常变化率大的区域大，改进后的矢量匹配算法的平均圆概率误 差比单点粒子滤波平均减小，并且在匹配区域增量平稳，说明改进后的矢量匹配算法对重 力特征变化具有较强的鲁棒性。
[0096] 以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人 员来说，在不脱离本发明原理的前提下，还可以做出若干改进，或者对其中部分技术特征进 行等同替换，这些改进和替换也应视为本发明的保护范围。
【主权项】
1. 一种基于粒子滤波的重力采样矢量匹配定位方法，其特征在于，包括如下步骤： 步骤一、根据水下载体的运动规律，将运载体的经炜度信息作为状态变量，重力仪实时 测量重力异常值为观测量，建立重力辅助惯导单点匹配模型：式中，Λ Xkik+1表示k时刻到k+Ι时刻输出的潜艇的经炜度差值，Λ U 惯导系统给出 k时刻的偏移增量，ek惯导系统的误差，y k表示k时刻的重力仪测量的重力异常信息，h k (Xk) 表示在xk处在重力基准图上读取的重力异常值，vk表示重力异常测量误差和重力基准图误 差； 步骤二、计算矢量匹配基准条件：设水下运载体为匀速航行，Pk表示运载体航行过程 中惯导系统输出的点k的位置信息，则等间隔采样点集{Pk 4, Pk 3, Pk 2, Pk i，PJ的矢量信息其中（mk，nk)表示k点惯导系统输出的经炜度 信息，感教表示无误差惯导系统两相邻采样点之间的欧氏距离，σ表示测量误差的上 限； 步骤三、序贯重要性采样：假设从后验概率密度P(XkIYk)中抽出N个独立同分布的随机 样本i = 1，···，Ν，则p(Xk|Yk)被表示成这些随机样本求和形式；引入一个已知、容易 采样的重要性概率密度函数q(xk|Yk)，从中生成采样粒子，实现求和逼近后验概率密度函 数P (xk I Yk)，则后验概率密度表示为从重要性密度函数采样的随机样本点与每个样本所对 应的权值乘积和的形式，粒子权值定义为重要性密度函数逼近后验概率函数的程度；从重 要性概率密度采样中生成采样粒子，并随着测量值的依次到来递推求得相应的权值，进而 得到状态估计； 步骤四：重采样：根据步骤三得到的粒子权值，计算有效粒子数次#来衡量粒子权值的 退化程度； 步骤五、对惯性导航系统的状态进行估计：根据现有的观测其中量估计出重力辅助惯 导定位系统的位置信息； 步骤六、判断单点匹配结果是否可信：单点匹配结果应满足惯导相邻点之间位相关性， 则单点匹配结果也应满足公式（3);若满足，则将匹配结果视为可信点，若不满足，则在点 匹配过程中加入i的限定的条件得到新的重要性函数，根据由式（3)所确 定的可信范围重新计算状态变量的转移概率密度函数，得到；进行步骤五，得到 新的匹配结果； 步骤七、计算矢量匹配结果权值：若匹配结果均不满足式（3)，则得到四个局部最优 解，利用加权最小二乘估计得到最终估计值 步骤八：按照步骤一中的Λ Uk更新p (Xk I Xk D，再根据步骤四更新的粒子权值，进行k+1 时刻的惯导位置估计，再进行步骤六、七，得到矢量匹配结果。2.如权利要求1所述的一种基于粒子滤波的重力采样矢量匹配定位方法，其特征在 于，进一步地，所述的估计包含预测和更新两个阶段，预测过程利用建立的系统模型预测状 态的先验概率密度，更新过程则利用最新的测量值对先验概率密度进行修正，得到后验概 率密度。
【专利摘要】本发明提出一种基于粒子滤波的重力采样矢量匹配定位方法，矢量匹配算法考虑匹配点之间的位置相关性，解决了传统匹配算法匹配结果可信度不高的问题。本发明利用重力采样点所组成的矢量进行匹配，对单点匹配结果的二次估计，利用基于贝叶斯估计的粒子滤波实现。在单点匹配之后，计算匹配结果相邻点之间的欧氏距离，判断其是否满足无误差惯导系统两相邻采样点之间的欧氏距离所确定的范围。若满足，则该点匹配结果可信；若不满足，则根据限定条件重新进行单点匹配。
【IPC分类】G01C21/20
【公开号】CN105180938
【申请号】CN201510300603
【发明人】王博, 于力, 肖烜, 邓志红
【申请人】北京理工大学
【公开日】2015年12月23日
【申请日】2015年6月3日