[发明专利]一种基于最小二乘和多重渐消因子的改进kalman滤波方法

权利要求书

1.一种基于最小二乘和多重渐消因子的改进kalman滤波方法，其特征在于，包括以下步骤：

步骤1、根据传感器测量信息，获取开始滤波前的时间序列p及其对应时刻的跟踪目标的位置测量值y_p，其中p＝1，2，...，m，m为滤波初始时刻，对位置测量值进行最小二乘拟合，计算滤波初值；

步骤2、分析跟踪目标运动特征及传感器的测量精度，建立关于跟踪目标的状态方程和观测方程；

步骤3、根据步骤2中得到的状态方程、观测方程各参数，及步骤1中得到的滤波初值进行改进的kalman滤波；改进的kalman滤波具体如下：计算状态预测向量；通过新息协方差计算得到多重渐消因子，并利用辅助函数对多重渐消因子进行修正处理，然后利用修正后的多重渐消因子计算预测误差协方差；计算滤波增益、状态估计向量及估计误差协方差；更新采样时刻，获得各采样时刻跟踪目标的状态向量；

步骤1中计算滤波初值，具体如下：

步骤1.1、利用最小二乘法对步骤1中采集到的时间序列及其对应的跟踪目标的位置测量值进行拟合，得到关于时间序列的二阶拟合方程；

建立二阶拟合方程f(t)和其对应的目标函数J：

f(t)＝β₀+β1₁t+β₂t² (1.1.1)

其中，β₀,β₁，β₂为拟合参数；

将时间序列p作为上述拟合方程的自变量t，代入拟合方程；在目标函数中，令变量y等于跟踪目标的位置测量值y_p，代入目标函数；

对目标函数的各拟合参数求导，并令其值为0，得到矩阵方程：

计算得到二阶拟合参数β₀，β₁，β₂；

将计算得到的拟合参数代入式(1.1.1)，得到符合跟踪目标运动轨迹的二阶拟合方程；

步骤1.2、利用步骤1.1中的拟合方程及滤波初始时刻p＝m，求取滤波初值；

将滤波初始时刻m作为自变量代入步骤1.1中得到的拟合方程，求解得到滤波初始时刻跟踪目标的位置信息f(m)；

对步骤1.1得到的拟合方程进行一阶求导，得到跟踪目标运动的速度方程v(t)；

将滤波初始时刻作为自变量代入速度方程v(t)，得到滤波初始时刻跟踪目标的速度信息v(m)；

步骤1.3、将步骤1.2中求得的m时刻跟踪目标的位置信息f(m)和速度信息v(m)组合，得到滤波初始时刻的状态估计向量

步骤2中建立关于跟踪目标的状态方程和观测方程，表达式如下：

X(k)＝Φ_k|k-1X(k-1)+W(k) (2.1)

Z(k)＝H_kX(k)+V(k) (2.2)

其中，k为当前滤波时刻，k＝m+1，…，N，N为最终的滤波采样时刻，X(k)为跟踪系统在当前滤波时刻的状态向量，Z(k)为跟踪系统在当前滤波时刻的观测向量，Φ_k|k-1为当前滤波时刻的状态转移矩阵，H_k为当前滤波时刻的观测矩阵，W(k)和V(k)分别为跟踪系统的过程噪声和量测噪声，且均为均值为0，当前滤波时刻协方差分别为Q_k和R_k的不相关白噪声，Q_k为过程噪声协方差，R_k为量测噪声协方差；

步骤3具体如下：

步骤3.1、利用步骤2中得到的状态转移矩阵计算状态预测向量具体公式如下：

其中，k为当前滤波时刻，k-1为上一滤波时刻，为上一滤波时刻的状态估计向量；

步骤3.2、根据步骤3.1得到的状态预测向量及步骤2中得到的观测矩阵和观测向量计算新息向量Y(k)，并利用上一时刻的估计误差协方差及标准kalman滤波框架下的预测误差协方差计算公式求解标准kalman框架下的新息协方差理论值C′(k)，具体公式如下：

其中，P′(k|k-1)为标准kalman滤波框架下的预测误差协方差，上标T为矩阵的转置，P(k-1|k-1)为上一滤波时刻的估计误差协方差，E[*]为期望值；

步骤3.3、利用步骤3.2中得到的新息向量及新息协方差理论值，计算实际新息协方差与新息协方差理论值的差值F(k)，具体公式如下：

F(k)＝Y(k)^TY(k)-γtrace[E[Y(k)Y(k)^T]] (3.3.1)

其中，γ为可调储存系数，γ≥1，trace[*]为矩阵的迹；

利用上述差值F(k)计算如下辅助函数S(k)，为求取多重渐消因子做准备，函数具体如下：

其中，arctan为反三角函数中的反正切函数；

步骤3.4、根据步骤3.2中的新息协方差理论值及新息向量求取多重渐消因子矩阵；

设当前滤波时刻对应的多重渐消因子λ_k为对角阵，即λ_k＝diag{λ_1k,λ_2k,...,λ_nk}，其中λ_ik对应第i个数据通道在k时刻的渐消因子的值，且满足λ_ik≥1，i＝1,2,…,n，n为预测误差协方差阵的维数；

设修正后的新息协方差C(k)与新息协方差理论值C′(k)满足如下关系：

C(k)＝α_kC′(k) (3.4.1)

其中，Y₀为m+1时刻的新息向量，I_n为n阶的单位矩阵，α_k为对角阵因子，即α_k＝diag{α_1k,α_2k，...,α_nk}，且满足α_ik≥1，α_ik为k时刻新息协方差理论值的第i个数据通道的参数，由步骤3.2中新息协方差和新息向量的计算方程及公式(3.4.1)、(3.4.2)计算对角阵因子，即

其中，c′_ijk为k时刻对应的新息协方差阵理论值C′(k)中第i行第j列元素的值，c_ijk为k时刻对应的修正计算后的新息协方差阵C(k)中第i行第j列元素的值；

在利用新息协方差计算多重渐消因子的过程中，认为对角阵因子与多重渐消因子矩阵等价，利用式(3.4.3)及步骤3.3中的辅助函数，计算当前滤波时刻的多重渐消因子；具体如下：

λ_k＝diag{max^S(k){1,c_1jk/c′_1jk},max^S(k){1,c_2jk/c′_2jk},...,max^S(k){1,c_njk/c′_njk}}；

其中，max^S(k){*}为最大值的S(k)次方；

步骤3.5、根据步骤3.4中计算得到的当前时刻的多重渐消因子λ_k及上一时刻的估计误差协方差求取预测误差协方差P(k|k-1)，具体如下：

步骤3.6、根据步骤3.1中的状态预测向量和步骤3.5中的预测误差协方差求取滤波增益、状态估计向量及估计误差协方差，公式如下：

P(k|k)＝(I_n-K(k)H_k)P(k|k-1)

其中，K(k)为滤波增益，为状态估计向量，P(k|k)为估计误差协方差；

步骤3.7、采样时刻更新，进行步骤3.1～3.6，直至采样时刻结束，以此得到更加可靠的各采样时刻跟踪目标的状态向量。

2.根据权利要求1所述的一种基于最小二乘和多重渐消因子的改进kalman滤波方法，其特征在于，γ＝1为最严格收敛判据。