[发明专利]一种基于加权最小二乘容积卡尔曼滤波的目标定位方法

权利要求书

1.一种基于加权最小二乘容积卡尔曼滤波的目标定位方法，所述方法包括：将最小二乘估计和容积卡尔曼滤波方法进行结合，借助于最小二乘估计将最新测量结果集成到每个容积粒子中，从而促进容积点从先验区域到高似然区域的移动；并对量测信息进行了扩维，从而计算每一时刻目标的状态和估计误差方差；

所述方法具体包括：

步骤1)获取k-1时刻目标的状态和估计误差方差P_k-1,k-1；以及k时刻的量测信息Z_k；

步骤2)k-1时刻目标的状态和估计误差方差P_k-1,k-1生成m个容积粒子，引入k时刻的量测信息Z_k对每个容积粒子上使用最小二乘估计进行优化，得到优化后的容积粒子值，然后根据容积卡尔曼滤波算法得到k时刻目标的状态和估计误差方差P_k,k；

步骤3)输出k时刻目标的状态和估计误差方差P_k,k。

2.根据权利要求1所述的基于加权最小二乘容积卡尔曼滤波的目标定位方法，其特征在于，所述k-1时刻目标的状态和估计误差方差P_k-1,l-1生成m个容积粒子，具体包括：

步骤2-1-1)对k-1时刻的目标的状态估计误差方差P_k-1,k-1进行分解得到：

式中S_k-1,k-1是对P_k-1,k-1做Cholesky分解得到的矩阵；

步骤2-1-2)计算每个cubature点

其中i＝1,2,…m，m＝2n_x，n_x为目标状态变量的维数；

步骤2-1-3)计算更新后的cubature点

其中，f(·)为状态转移函数。

3.根据权利要求2所述的基于加权最小二乘容积卡尔曼滤波的目标定位方法，其特征在于，所述引入k时刻的量测信息Z_k对每个容积粒子上使用最小二乘估计进行优化，得到优化后的容积粒子值；具体包括：

步骤2-2-1)从每一个由步骤2-1-3)计算得出的容积点中提取出对量测信息Z_k有贡献的元素，记为求出对应的雅克比矩阵

步骤2-2-2)计算优化后的值x^*；

若不考虑权重信息，则：

其中，I变为n_p×n_p的单位矩阵，n_p是的长度，其中，h(·)为量测信息函数，λ的定义为：

若考虑权重信息，权重矩阵W是一个对角矩阵，对角线上的元素是各个量测误差值对应的权重；则：

步骤2-2-3)用步骤2-2-2)的x^*第二次更新得到优化后的容积粒子值。