[发明专利]一种应用于非线性高斯模型的平滑约束扩展卡尔曼滤波方法

权利要求书

1.一种应用于非线性高斯模型的平滑约束扩展卡尔曼滤波方法，其特征在于，包括如下步骤：

步骤一：建立约束离散非线性随机系统

对于非线性随机系统的状态估计问题，离散时间状态模型包括系统状态过程模型和系统测量模型，其中，系统状态过程模型将当前时刻状态向量和前一时刻状态向量相关联，系统测量模型将状态向量和测量向量相关联，具体为：

X_k+1＝F_k(X_k)+V_k (1)

Z_k＝H_k(X_k)+e_k (2)

式(1)-(2)中，k是跟踪时刻；表示k时刻n_X维的系统状态向量；是k时刻n_Z维的量测向量；F_k与H_k分别表示已知的状态转移矩阵和测量矩阵；V_k和e_k分别是过程噪声和测量噪声；

对于物理意义上系统统计噪声，数学上，根据最小二乘方法优化状态估计，即：

式(3)中，K表示系统观测总时长；

步骤二：约束正则化非线性随机系统可行域

给定测量序列Z_k，通过递推估计得到目标状态的最大后验分布p(X_k|Z_1:k)，进而得到状态向量满足约束条件的最大后验概率估计，为：

式(4)中，上标c表示约束，ln表示取自然对数，为第k时刻目标状态后验概率密度函数准确的有效区域；

根据对偶原理，最大化等价于最小化因此得到状态向量的可行域中心点的目标函数为：

式(5)中，i表示第i个估计，分别是-logp(X_k|Z_1:k)在点处的雅可比和海森矩阵；

将状态变量倒数的自然对数为障碍函数，对状态向量的可行域中心点的目标函数进行正则化，将状态向量的可行域中心点的目标函数转化为序列无约束优化增广目标函数，即：

式(6)中，X_k为状态变量，ρ∈(0,1)是罚函数因子；

基于序列无约束优化增广目标函数得到状态向量的可行域：

式(7)中，为可行域的均值，P_XX为可行域的协方差；

步骤三，约束非线性系统更新

联立式子(1)和(7)，传递状态可行域

在可行域内，状态传递过程的雅可比为：

式(9)-(10)中，表示约束条件下的状态转移矩阵，G_k表示系统过程噪声的雅可比，f_k表示系统状态传递函数，表示约束条件下，可行状态集的传递；

由下式计算约束条件下新息协方差为：

式(11)中，Q_k表示第k时刻的过程噪声的标准方差；

在可行域内，测量模型的雅可比为：

式(12)-(13)中，表示约束条件下的测量矩阵，U_k表示系统观测噪声的雅可比，h_k表示系统观测方程，表示系统观测噪声均值；

得到约束条件下卡尔曼滤波增益为：

式(14)中，R_k表示第k时刻的量测噪声的标准方差；

得到状态更新的均值和协方差，为：

2.根据权利要求1所述应用于非线性高斯模型的平滑约束扩展卡尔曼滤波方法，其特征在于，步骤一中，过程噪声和测量噪声为互不相关的高斯噪声，均值为零。

3.根据权利要求2所述应用于非线性高斯模型的平滑约束扩展卡尔曼滤波方法，其特征在于，步骤二中，第k时刻目标状态后验概率密度函数准确的有效区域定义为：

式中，是d_z维连通的量测区域。