[发明专利]一种基于采样点调整的无迹卡尔曼滤波方法

权利要求书

1.一种基于采样点调整的无迹卡尔曼滤波方法，其特征在于，包括以下步骤：

1)指定初始时刻系统状态变量x₀的均值协方差阵P₀；

2)根据K时刻的观测量、K-1时刻的状态变量x_k-1和协方差阵P_x,k-1，结合无迹卡尔曼数据模型及三阶精度条件，估计更新K时刻的系统状态量和协方差阵P_x,k；

3)将K时刻增加1，返回步骤2)迭代。

2.如权利要求1所述的基于采样点调整的无迹卡尔曼滤波方法，其特征在于，步骤1)的具体步骤如下：

1-1)指定初始时刻状态变量x的先验初始分布为x₀，初始时刻的状态变量x₀为服从均值为x₀、协方差阵为P₀的高斯分布的随机抽样。

3.如权利要求1所述的基于采样点调整的无迹卡尔曼滤波方法，其特征在于，步骤2)所述结合无迹卡尔曼数据模型及三阶精度条件估计更新K时刻的系统状态量和协方差阵P_x,k的具体方法为：

2-1-1)对采集到的非线性观测值建模，该模型包括有数据的状态方程和观测方程；选取出Sigma点通用形式，计算通用形式Sigma点的相应加权值，形成部分Sigma点集，根据加权值计算出所建模型保持三阶精度的条件；

2-1-2)结合步骤2-1-1)中所述三阶精度条件，调整部分Sigma点集的权重和方向，形成新Sigma点集。

4.如权利要求3所述的基于采样点调整的无迹卡尔曼滤波方法，特征在于，步骤2-1-1)的具体方法如下：

2-1-1-1)对采集到的非线性观测值建模，得到非线性观测值的空间模型，其状态方程和观测方程为：

x_k+1＝f(x_k)+v_k

y_k＝h(x_k)+w_k

其中，x_k和y_k分别表示k时刻系统的状态向量和观测向量，f(·)和h(·)为非线性向量函数，v_k和w_k分别是过程高斯噪声和观测高斯噪声，均值为零、协方差各为Q_k和R_k且互不相关；

2-1-1-2)选取的Sigma点通用格式为：

其中ω_i和S_a＝[s_a,1,s_a,2,…,s_a,n]^T分别表示采样点的大小和方向，[·]^T表示矩阵转置，为状态向量x的均值，采样点个数为M+1；

2-1-1-3)观测量即输出y＝g(x)＝[g₁(x),g₂(x),…,g_m(x)]^T，在无迹卡尔曼滤波中，用Sigma点的加权和表示系统观测量均值：

其中E(·)为数学期望，Wⁱ表示Sigma点的权重值；

2-1-1-4)根据步骤2-1-1-3)中系统观测值均值E(g_i(x))，提出所建保持三阶精度的充分条件如下：

(1)M＝2Zn(Z∈{1,2,3,…})，n为状态变量x的维数；

(2)ω_2(r-1)n+i＝-ω_(2r-1)n+i(i＝1,2,…,n,r＝1,2,…,Z)；

(3)令Λ^2r-1＝[S_2(r-1)*n+1,S_2(r-1)*n+2,…,S_2(r-1)*n+n]且Λ^2r＝[S_(2r-1)*n+1,S_(2r-1)*n+2,…，S_(2r-1)*n+n]，有Λ^2r-1＝Λ^2r，Λ^2r-1(Λ^2r-1)^T＝P_x。