扩展卡尔曼滤波matlab(一种改进的扩展卡尔曼滤波)

状态方程是：[X(k ^ 1)=f(k，X(k)) V(k)] (1)

测量公式为：[Z(k 1)=h(k，X(k)) W(k)] (2)

其中：[X(k)]是目标的状态向量；[V(k)]是过程噪声，方差[E[V(k)V '(j)]=Q(k)kj]；[Z(k)]是时间k的测量向量；[W(k)]是方差[E(W(k)W '(j)]=R(k)kj]的测量噪声；[f(k)]是状态转移矩阵；[h(k)]是测量矩阵。目标运动模型是辛格模型：[f=1t(t-1e-t)201(1-e-t)00e-t]

[h=[1 0 0]]状态方程中函数的一阶展开式为：

[X(k 1)=f(k，X(kk)FX(k)[X(k)-X(kk)]v(k)](3)扩展卡尔曼滤波过程如下：

状态的一步预测方程是：[X(k ^ 1k)=f[k，X(kk)](4)

预测方程为：[P(K1K)=FX(K)P(KK)FX '(K)Q(K)](5)

测量预测方程为：[Z(k 1k)=h[k 1，X(k 1k)](6)

方差创新协会是[s(k1)=hx(k1)p(k1 k)hx '(k1)r(k1)](7)

增益为[K(K 1)=P(K 1k)hx’(K 1)S-1(K 1)](8)

状态更新方程为：[X(K1k)=X(K1k)K(K1){ Z(K1)-H[K1，X(K1k)]](9)

更新公式为：[P(K1)=[I-K(K1)HX(K1)]P(K1)[I K(K1)HX(K1)]-K(K1)R(K1)K '(K1)](10)

该算法只能通过给出初始状态值和滤波器估计状态向量的co-方差matrix来启动和循环[6]。2改进算法

在系统达到稳定状态后，预测的合作、创新和收益都趋于最小[6]。如果此时目标变化强烈，预测值不再准确，创新变大，但增益不变，导致滤波精度下降；建立的运动模型和目标的实际运动模型之间的不匹配是由于不准确的预测值，这导致不准确的滤波值。由于增益矩阵K的计算量很大，重新计算增益矩阵K会导致实时性下降。因此，本文通过一步提高预测值来提高滤波值的准确性。滤波趋于稳态后，新息矩阵趋于最小值，因此可以判断预测值是否应该根据新息矩阵：的大小进行调整

[r(k 1)zmax] (11): [zmax]是测量误差矩阵的最大值；[]是矩阵的f？常态。[]的取值范围是[[0.3，0.9]]。当新息矩阵满足公式(11)时，滤波被认为是正常的。当等式(11)不成立时，认为目标具有强机动。此时，测量值的可靠性较高，一步预测值的可靠性较低，因此应修正一步预测值。

如果等式(11)不满足，则表明一步预测值[X(k 1k)]不准确，因此修改[X(k 1k)]并通过重新计算状态更新等式获得新的[x (K1K)]。具体校正方法如下：[x(K1K)x(K1K)=x(K1K)x(K1K)r’(K1)* C1C2T]

其中：[c1]为对；预测位置的修正值，[c2]为对预测速度的修正值，[R’(k 1)]为新息矩阵在k1时刻的距离分量。[c1]和[c2]的取值范围为[(0，1)]，在工程应用中应根据实际情况进行设置。3对真理的模仿

通过设置以下两个实验场景，对EKF算法和本文提出的改进算法进行了比较：(1)机动目标匀速直线运动一段时间(20秒)，然后加速20秒，再匀速运动60秒。

(2)机动目标在不同时间段匀速、转弯和恒速运动，运动持续时间分别为80 s、40 s和40 s。实验结果通过比较目标位置的RMSE和对3.1仿真1:匀速运动和加速运动来比较两种算法的准确性

目标起始位置为：[x (k)=2 000，2 00，0，1 000，150，0t]

目标移动时间为100秒，采样间隔为2秒；系统的噪声系数[q=0 . 010 . 01 0 . 010 . 01 0 . 010 . 01]；测量噪声矩阵[R=1002001002]。从图1可以看出，滤波算法稳定后，两种算法的位置RMSE相似；然而，当目标有较强的机动变化时，EKF滤波的精度大大降低，该算法的精度更高。由于目标的强机动，测量值与一步预测值之间的差距突然增大，但滤波不能随之改变，导致扩展卡尔曼滤波精度较低。改进的滤波算法可以根据新息矩阵范数实时调整估计值，从而提高滤波算法的精度。

3.2模拟2:匀速运动和转弯运动中目标的起始位置为：

[X(k)=2 000，2 00，0，1 000，150，0T]目标的移动时间为160 s，采样间隔为2 s。系统噪声方差[q=0 . 010 . 01 0 . 010 . 01 0 . 010 . 01]；测量噪声co-方差[r=1002001002]。

图1 RMSE(位置)的不同算法(一)

从图2可以看出，当匀速直线运动时，扩展卡尔曼滤波算法的精度与该算法相似，达到稳态所需的时间几乎相同。当目标机动转弯时，由于一步预测值与实测值相差较大，EKF算法的精度大大降低。改进算法根据新息矩阵，检测不稳定滤波，通过修改一步预测值提高滤波精度。上述情况表明，当机动目标突变或运动模型建立不准确时，改进算法通过修正预测值提高了滤波算法的精度。

图2 RMSE(位置)(2)不同算法的4个子句

EKF算法已广泛应用于机动目标跟踪，并取得了良好的效果。然而，在实际工程应用中，机动目标的运动状态是不稳定的，会发生各种变化。然而，这种变化将导致过滤精度显著降低。鉴于本文对对，对EKF算法进行了深入的研究，通过新息矩阵范数来判断滤波是否稳定，如果不稳定，通过修改预测值来提高滤波值的准确性。仿真实验表明，通过改进的对比算法和EKF算法在不同场景下的精度，改进的算法提高了算法的精度，能够更好地适应机动目标跟踪过程。