扩展卡尔曼滤波算法及仿真实例

在阅读本篇博客之前希望读者已经具备线性卡尔曼滤波器的基础，或者提前研读我的前一篇关于线性卡尔曼滤波器的文章：线性卡尔曼滤波算法及示例。下面不说废话，直奔主题了。

一、扩展卡尔曼滤波器（EKF）理论基础

扩展Kalman滤波器算法实质上是一种在线线性化技术，即安装估计轨道进行线性化处理—-泰勒级数展开，再进行线性的Kalman滤波。实际非线性滤波处理，通常对过程噪声和观测噪声近似为高斯分布，协方差分别为 Qwk Q w k 和 Qvk Q v k ，均值假定为0.

考虑离散时间非线性动态系统：

{xk+1=fk(xk,wk)zk=hk(xk,vk) { x k + 1 = f k ( x k , w k ) z k = h k ( x k , v k )

其中：
wk w k 为过程演化噪声
vk v k 为量测噪声

符号说明： xk x k ：为真值， x^k x ^ k ：为状态估计值 ， x~k x ~ k ：为状态预测误差值，他们之间的关系是： xk=x^k+x~k x k = x ^ k + x ~ k 。

一阶EKF算法的步骤描述如下：
1、在时刻 k−1 k − 1 ，假定已经获得上一时刻状态估计值和估计误差的协方差阵：

x^k−1|k−1,Pk−1|k−1 x ^ k − 1 | k − 1 , P k − 1 | k − 1

现在需要根据上一时刻的状态值和离散非线性动态系统数学模型估计下一时刻先验估计值，首先对系统模型线性化处理，处理过程如下：

xk=fk−1(xk−1,wk−1)=fk−1((x^k−1|k−1+x~k−1|k−1),wk−1) x k = f k − 1 ( x k − 1 , w k − 1 ) = f k − 1 ( ( x ^ k − 1 | k − 1 + x ~ k − 1 | k − 1 ) , w k − 1 )

注意上式中下一时刻是关于前一时刻的函数，即将前一时刻状态作为自变量，后一时刻作为因变量（注意到这点对后面工程运用中处理数学模型问题非常重要），因此自变量前后两个时刻之间相差一个 x~k−1|k−1=xk−1−x^k−1|k−1 x ~ k − 1 | k − 1 = x k − 1 − x ^ k − 1 | k − 1 ，对其安照一阶泰勒展开可得如下式子：

xk=fk−1(xk−1,wk−1)=fk−1((x^k−1|k−1+x~k−1|k−1),wk−1)≈fk−1(x^k−1|k−1,0)+fxk−1x~k−1|k−1+fwk−1wk−1(1.1.1) (1.1.1) x k = f k − 1 ( x k − 1 , w k − 1 ) = f k − 1 ( ( x ^ k − 1 | k − 1 + x ~ k − 1 | k − 1 ) , w k − 1 ) ≈ f k − 1 ( x ^ k − 1 | k − 1 , 0 ) + f k − 1 x x ~ k − 1 | k − 1 + f k − 1 w w k − 1

注意到 x~k−1|k−1 x ~ k − 1 | k − 1 相当于我们在数学课本中见到的 Δx Δ x ， x^k−1|k−1 x ^ k − 1 | k − 1 相当于数学课本中的 x0 x 0 ， xk−1 x k − 1 相当于课本中的下一时刻值 x x 。

x~k−1|k−1=xk−1−x^k−1|k−1 x ~ k − 1 | k − 1 = x k − 1 − x ^ k − 1 | k − 1 类比于 Δx=x−x0 Δ x = x − x 0
上式之所以成立解释如下：
xk−1 x k − 1 为上一时刻的真值，实际上该值我们是无法获得的，上一时刻我们所用的是后验值 x^k−1|k−1 x ^ k − 1 | k − 1 来近似真值 xk−1 x k − 1 ，其中的误差即为 x~k−1|k−1 x ~ k − 1 | k − 1
泰勒展开式中两个偏导分别为如下：

fxk−1=∂fk−1(xk−1,wk−1)∂xk−1∣∣∣xk−1=x^k−1|k−1,wk−1=0 f k − 1 x = ∂ f k − 1 ( x k − 1 , w k − 1 ) ∂ x k − 1 | x k − 1 = x ^ k − 1 | k − 1 , w k − 1 = 0

fwk−1=∂fk−1(xk−1,wk−1)∂wk−1∣∣∣xk−1=x^k−1|k−1,wk−1=0 f k − 1 w = ∂ f k − 1 ( x k − 1 , w k − 1 ) ∂ w k − 1 | x k − 1 = x ^ k − 1 | k − 1 , w k − 1 = 0

由此上式非线性系统就近似为了线性系统。
2、现在通过k-1时刻的状态值对时刻k进行一步提前预测：
注意到我们的数学模型中，后一时刻是前一时刻的函数，因此将前一时刻的值带入函数 fk−1 f k − 1 中就可以的得到对当前时刻的一步先验预测值。模型中我们写的前后时刻的两个值为真值，而真值在实际情况下是无法获取的，在前一时刻我们用的是后验状态估计值 x^k−1|k−1 x ^ k − 1 | k − 1 代替真值，因此在对下一步进行提前预测也要用该值。

x^k|k−1≈fk−1(x^k−1|k−1,0)(1.2.1) (1.2.1) x ^ k | k − 1 ≈ f k − 1 ( x ^ k − 1 | k − 1 , 0 )

该式相当于线性kalman中的 x^k|k−1=F∗x^k−1|k−1 x ^ k | k − 1 = F ∗ x ^ k − 1 | k − 1 两者的区别是后者有现成状态转移矩阵 F F ,前者需要通过其他方式求出。

提前一步状态预测所产生的误差为：

x~k|k−1=xx−x^k|k−1≈fxk−1x~k−1|k−1+fwk−1wk−1 x ~ k | k − 1 = x x − x ^ k | k − 1 ≈ f k − 1 x x ~ k − 1 | k − 1 + f k − 1 w w k − 1

上式可由式(1.1.1)和(1.2.1)合并求得。注意式中是真值与先验状态之间的误差关系，一开始前面演算的是真值与后验状态之间的误差关系。状态预测误差的协方差阵是：

Pk|k−1=cov(x~k|k−1)≈fxk−1Pk−1|k−1(fxk−1)T+fwk−1Qk−1(fwk−1)T P k | k − 1 = c o v ( x ~ k | k − 1 ) ≈ f k − 1 x P k − 1 | k − 1 ( f k − 1 x ) T + f k − 1 w Q k − 1 ( f k − 1 w ) T

3、k时刻量测的线性化方程为：

zk=hk(xk,vk)=hk(x^k|k−1+x~k|k−1,vk)≈hk(x^k|k−1,0)+hxkx~k|k−1+hvkvk z k = h k ( x k , v k ) = h k ( x ^ k | k − 1 + x ~ k | k − 1 , v k ) ≈ h k ( x ^ k | k − 1 , 0 ) + h k x x ~ k | k − 1 + h k v v k

其中

x~k|k−1=xk−x^k|k−1 x ~ k | k − 1 = x k − x ^ k | k − 1

hxk=∂hk(xk,vk)∂xk∣∣∣xk=xk|k−1^ h k x = ∂ h k ( x k , v k ) ∂ x k | x k = x k | k − 1 ^

hvk=∂hk(xk,vk)∂vk∣∣∣xk=xk|k−1^ h k v = ∂ h k ( x k , v k ) ∂ v k | x k = x k | k − 1 ^

4、对时刻k量测的一步提前预测
根据数学模型中的关于量测函数可知，量测真值是关于估计真值的函数，但在实际情况下，估计真值是无法获取的，在不存在噪声和线性化误差的情况下，通过数学模型所计算处理的值和直接通物理方式(在没有测量噪声的情况下)测量出来的真值是相等的，因为数学模型的线性化误差加先验估计值与估计真值直接存在一定误差，所有通过数学模型得出的测量值与实际测量真值是存在一定误差的。所通过数学模型计算出的值用先验测量值表示如下：

z^k|k−1≈hk(x^k|k−1,0) z ^ k | k − 1 ≈ h k ( x ^ k | k − 1 , 0 )

所有量测预测误差为：

z~k|k−1=zk−z^k|k−1≈hxkx~k|k−1+hvkvk z ~ k | k − 1 = z k − z ^ k | k − 1 ≈ h k x x ~ k | k − 1 + h k v v k

就上面表达式而言，如果 zk z k 为量测真值，那么 z~k|k−1 z ~ k | k − 1 就只是包含有预测误差在里面，但在实际运用过程中， zk z k 是无法获取的，我们只是用带有噪声的实际物理测量值代替真值，因此上述误差 z~k|k−1 z ~ k | k − 1 实际上就表示为预测误差和噪声误差之和。

量测预测误差的协方差阵为

Rz~k|k−1z~k|k−1=cov(z~k|k−1)≈hxkPk|k−1(hxk)T+hvkRk(hvk)T R z ~ k | k − 1 z ~ k | k − 1 = c o v ( z ~ k | k − 1 ) ≈ h k x P k | k − 1 ( h k x ) T + h k v R k ( h k v ) T

状态预测误差与量测预测误差协方差阵为：

Rx~k|k−1z~k|k−1=cov(x~k|k−1,z~k|k−1)≈Pk|k−1(hvk)T R x ~ k | k − 1 z ~ k | k − 1 = c o v ( x ~ k | k − 1 , z ~ k | k − 1 ) ≈ P k | k − 1 ( h k v ) T

5、在时刻k得到新的量测 zk z k ,状态滤波的跟新公式为：

后验状态更新公式：

x^k|k=x^k|k−1+K(zk−hxkx^k|k−1) x ^ k | k = x ^ k | k − 1 + K ( z k − h k x x ^ k | k − 1 )

后验协方差更新公式：

Pk|k=(I−Khxk)Pk|k−1 P k | k = ( I − K h k x ) P k | k − 1

其中k时刻卡尔曼增益K为

K=Rx~k|k−1z~k|k−1Rx~k|k−1x~k|k−1=Pk|k−1(hxk)ThxkPk|k−1(hxk)T+hvkRk(hvk)T K = R x ~ k | k − 1 z ~ k | k − 1 R x ~ k | k − 1 x ~ k | k − 1 = P k | k − 1 ( h k x ) T h k x P k | k − 1 ( h k x ) T + h k v R k ( h k v ) T

6、扩展卡尔曼算法执行过程：

• 状态计算分成——时间更新和滤波更新

  时间更新： x^k|k−1≈fk−1(x^k−1|k−1,0) x ^ k | k − 1 ≈ f k − 1 ( x ^ k − 1 | k − 1 , 0 )
  滤波更新： x^k|k=x^k|k−1+K(zk−hxkx^k|k−1) x ^ k | k = x ^ k | k − 1 + K ( z k − h k x x ^ k | k − 1 )
  K=Rx~k|k−1z~k|k−1Rx~k|k−1x~k|k−1=Pk|k−1(hxk)ThxkPk|k−1(hxk)T+hvkRk(hvk)T K = R x ~ k | k − 1 z ~ k | k − 1 R x ~ k | k − 1 x ~ k | k − 1 = P k | k − 1 ( h k x ) T h k x P k | k − 1 ( h k x ) T + h k v R k ( h k v ) T

• 方差计算分成——时间更新和滤波更新
  时间更新： Pk|k−1=cov(x~k|k−1)≈fxk−1Pk−1|k−1(fxk−1)T+fwk−1Qk−1(fwk−1)T P k | k − 1 = c o v ( x ~ k | k − 1 ) ≈ f k − 1 x P k − 1 | k − 1 ( f k − 1 x ) T + f k − 1 w Q k − 1 ( f k − 1 w ) T
  滤波更新： Pk|k=(I−Khxk)Pk|k−1 P k | k = ( I − K h k x ) P k | k − 1

二、线性卡尔曼与扩展卡尔曼比较

对比两个线性卡尔曼与扩展卡尔曼算法的五个公式可以看出有两点不同：
1、状态计算中的时间更新：
线性卡尔曼： x^k|k−1=Fk−1x^k−1|k−1 x ^ k | k − 1 = F k − 1 x ^ k − 1 | k − 1
扩展卡尔曼： x^k|k−1≈fk−1(x^k−1|k−1,0) x ^ k | k − 1 ≈ f k − 1 ( x ^ k − 1 | k − 1 , 0 )
此处不同主要是根据具体运用对象不同而导致所建立的数学模型不同。前者的状态为线性转移，后者的状态为非线性转移。
2、方差计算中的时间更新：
线性卡尔曼：
Pk|k−1=Fk−1Pk−1FTk−1+Γk−1Qwk−1ΓTk−1 P k | k − 1 = F k − 1 P k − 1 F k − 1 T + Γ k − 1 Q k − 1 w Γ k − 1 T
扩展卡尔曼：
Pk|k−1=cov(x~k|k−1)≈fxk−1Pk−1|k−1(fxk−1)T+fwk−1Qk−1(fwk−1)T P k | k − 1 = c o v ( x ~ k | k − 1 ) ≈ f k − 1 x P k − 1 | k − 1 ( f k − 1 x ) T + f k − 1 w Q k − 1 ( f k − 1 w ) T
此处不同主要表现在状态转移矩阵上的区别，前者的状态转移矩阵是直接来源于一开始建立的数学模型，将前一时刻的后验状态值直接转移预测下一时刻的先验状态值，后者的是将非线性数学模型经过泰勒展开线性化后对误差的转移：

x~k|k−1=xx−x^k|k−1≈fxk−1x~k−1|k−1+fwk−1wk−1 x ~ k | k − 1 = x x − x ^ k | k − 1 ≈ f k − 1 x x ~ k − 1 | k − 1 + f k − 1 w w k − 1

因此两者转移的对象不同。

更进一步分析其实可以得出状态转移矩阵两者之间的近似关系：

Fk−1≈E+fxk−1 F k − 1 ≈ E + f k − 1 x

解释如下：对于线性卡尔曼在不考虑噪声部分的情况下。

xk=Fk−1xk−1|k−1=E∗xk−1|k−1+Fxk−1ϕ(2.2.1) (2.2.1) x k = F k − 1 x k − 1 | k − 1 = E ∗ x k − 1 | k − 1 + F k − 1 x ϕ

该公式的意义表示当前时刻值为前一时刻值加上前后两时刻值之间的变化量， 扩展卡尔曼在实际应用中就是按照该方式实行的。只有这样才能表现出后一时刻是前一时刻的函数形式。只是在线性系统里面等号右边两项可以合并，而在非线性系统中无法合并。对于扩展卡尔曼后面就是对（2.2.1）式进行处理求出对误差转移的转移矩阵。
注：此公式为本人杜撰的，没有严格的理论根据证明，只是为了方便自己记忆。

三、扩展卡尔曼应用实例

不管是线性卡尔曼还是非线性卡尔曼，他们的数学模型都有一个共同的特点，那就是后一时刻的状态是前一时刻状态的函数，而在实际应用场景中，我们是很难直接列写出这样的模型的，一般都是直接关于时间或者间接变量的函数模型。对于卡尔曼的标准模型在连续时间下就是我们数学里面的微分方程形式(那种不包含x只有y的微分方程形式)。因为很多时候我们无法直接获得该模型，因而一般用（2.2.1）式等号最右边代替非线性状态转移方程。

例子1：考虑一个在二维平面运动的机器人，已知该机器人只有前向（x方向）速度 Vt V t (不固定，可实时变化，但可以直接测量)和旋转角速度 wt w t 同速度一样，非定值，可实时测量获得。以上俩个变量可通过加速度计和陀螺仪测量。其示意图如下：

求解小车轨迹。
解：
首先根据题目内容建立数学模型：

⎧⎩⎨⎪⎪x˙t=Vtcosϕty˙t=Vtsinϕtϕ˙t=wt { x ˙ t = V t c o s ϕ t y ˙ t = V t s i n ϕ t ϕ ˙ t = w t

状态向量

X=⎡⎣⎢xtytϕt⎤⎦⎥ X = [ x t y t ϕ t ]

这里需要注意一点的是关于状态向量的选取，一般而言对于求取小车的轨迹，只需要知道(x,y)坐标即可，因此很多人选取状态状态向量为

X=[xtyt] X = [ x t y t ]

事实证明这样的状态向量是没法对后面转移矩阵推导的。至于为什么要加 ϕ ϕ 进去后面再解释。

下面将上述微分方程写成差分形式

⎧⎩⎨⎪⎪⎪⎪x^t+1=x^t+(Vt+wVt)Δtcosϕ^ty^t+1=y^t+(Vt+wVt)Δtsinϕ^tϕ^t+1=ϕ^t+(wt+wwt)Δt(3.1.1) (3.1.1) { x ^ t + 1 = x ^ t + ( V t + w V t ) Δ t c o s ϕ ^ t y ^ t + 1 = y ^ t + ( V t + w V t ) Δ t s i n ϕ ^ t ϕ ^ t + 1 = ϕ ^ t + ( w t + w w t ) Δ t

上式差分方程等号左边为下一时刻的状态量，等式右边第一项为前一时刻状态量，第二项为前后两时刻之间的变化量。因此上述差分方程组后一时刻状态是前一时刻状态的函数。即和标准的卡尔曼数学模型一样。即后一时刻为前一时刻和噪声的函数，下面就可以按照扩展卡尔曼理论公式进行后面的推导了。

还记得前面的关于线性卡尔曼和非线性卡尔曼关于状态转移矩阵的区别吗？
线性卡尔曼是对状态的转移，非线性卡尔曼是对状态误差的转移。
状态误差：

⎧⎩⎨⎪⎪x~t+1=xt+1−x^t+1y~t+1=yt+1−y^t+1ϕ~t+1=ϕt+1−ϕ^t+1 { x ~ t + 1 = x t + 1 − x ^ t + 1 y ~ t + 1 = y t + 1 − y ^ t + 1 ϕ ~ t + 1 = ϕ t + 1 − ϕ ^ t + 1

误差提前一步预测公式：

x~k|k−1=xx−x^k|k−1≈fxk−1x~k−1|k−1+fwk−1wk−1 x ~ k | k − 1 = x x − x ^ k | k − 1 ≈ f k − 1 x x ~ k − 1 | k − 1 + f k − 1 w w k − 1

通过对(3.1.1)式求偏导可分别求出 fxt−1 f t − 1 x 和 fwt−1 f t − 1 w

fxt−1=⎡⎣⎢⎢100010−VmΔtsinϕ^VmΔtcosϕ^1⎤⎦⎥⎥ f t − 1 x = [ 1 0 − V m Δ t s i n ϕ ^ 0 1 V m Δ t c o s ϕ ^ 0 0 1 ]

fwt−1=⎡⎣⎢ΔtcosϕΔtsinϕ000Δt⎤⎦⎥ f t − 1 w = [ Δ t c o s ϕ 0 Δ t s i n ϕ 0 0 Δ t ]

之前提到过为什么要将状态向量写成：

X=⎡⎣⎢xtytϕt⎤⎦⎥ X = [ x t y t ϕ t ]

而不是直接写成：

X=[xtyt] X = [ x t y t ]

如果只是写成下面这种方式的话，那么在求状态转移据的偏导过程中，毫无疑问状态转移矩阵就是一个单位矩阵，为线性系统，对于非线性系统是不准确的，只有写成第一种方式，才能继续我们后面的推导。

注：这里提一下关于状态向量的选取经验：

1. 基本上大部分工程背景都是关于时间的系统，不管是对于线性还是非线性卡尔曼，他们的数学模型最终都是关于时间的函数(这也是我们最容易根据实际对象背景列写出来的)，而在公式推导过程中，我们希望它是一个后一时刻是关于前一时刻的函数，因此才有了我们上面的差分方程形式。对于怎样将关于时间的函数写成关于前一时刻的差分方程形式，通过一阶泰勒展开即可。
2. 有了差分方程，我们就可以求状态转移矩阵，状态转移矩阵是系统方程对状态向量的偏导数，为了保证系统的非线性，状态向量之间不能都是独立的变量，否则状态转移矩阵就是单位矩阵了，而对于我们选取的
   X=[xtyt] X = [ x t y t ]
   
   两变量之间是相互独立的，所以不行。
   对于：
   
   X=⎡⎣⎢xtytϕt⎤⎦⎥ X = [ x t y t ϕ t ]
   
   前两个变量都是关于第三个变量的函数。不存在相互独立，所以可以。

由上可得误差状态转移方程：

⎡⎣⎢x~t+1y~t+1ϕ~t+1⎤⎦⎥=⎡⎣⎢⎢100010−VmΔtsinϕ^VmΔtcosϕ^1⎤⎦⎥⎥⎡⎣⎢x~ty~tϕ~t⎤⎦⎥+⎡⎣⎢ΔtcosϕΔtsinϕ000Δt⎤⎦⎥[wVtwwt] [ x ~ t + 1 y ~ t + 1 ϕ ~ t + 1 ] = [ 1 0 − V m Δ t s i n ϕ ^ 0 1 V m Δ t c o s ϕ ^ 0 0 1 ] [ x ~ t y ~ t ϕ ~ t ] + [ Δ t c o s ϕ 0 Δ t s i n ϕ 0 0 Δ t ] [ w V t w w t ]

简化形式

X~t+1=FtX~t+GtWt X ~ t + 1 = F t X ~ t + G t W t

通过上式就可以写出标准先验协方差更新公式：

P−t+1=AtPtATt+GtQtGTt P t + 1 − = A t P t A t T + G t Q t G t T

对于预测部分到此结束，关于量测部分需要根据量测的方式不同建立不同的数学模型按照同样的方式进行推导最后求解，这里就不再赘述。下面讲解一个实际的仿真例子。

例子2、设一平面目标状态向量为X=(x,y),以坐标原点为圆心，半径为500米进行匀速圆周运动，速度为10m/s；目标运动的过程噪声方差为2 m2 m 2 的白噪声。现有一个位于（750，750）的雷达覆盖该目标，进行监视，观测量为径向距离，噪声为3 m2 m 2 ,监视时间间隔为0.5秒。
针对以上场景进行目标跟踪滤波设计和仿真？

解：已知 v=10m/s v = 10 m / s ,可以算出角速度 w=v/r=10/500(rad/s) w = v / r = 10 / 500 ( r a d / s ) ;
根据题意及以上参数可得出雷达所测的径向距离：

dt=ft(θt)=((Xr−rcosθt)2+(Yr−rsinθt)2)1/2 d t = f t ( θ t ) = ( ( X r − r c o s θ t ) 2 + ( Y r − r s i n θ t ) 2 ) 1 / 2

其中 θ˙=wt θ ˙ = w t ， Xr=750 X r = 750 , Yr=750 Y r = 750
定义状态向量：

Xt=[dtθt] X t = [ d t θ t ]

状态向量中两个分量是相关的，第一个分量是第二个分量的函数。
将状态向量以差分方式表示为：
因为径向距离是直接关于 θ θ 的函数，因此可对其直接用一阶泰勒展开:

dt+1≈dt+∂f∂θΔθ=dt+fθtwT d t + 1 ≈ d t + ∂ f ∂ θ Δ θ = d t + f t θ w T

其中 T T 为监视时间间隔为0.5.
因而状态向量的差分方程组形式为：

{d^t+1=d^t+fθtwTθ^t+1=θ^t+wT { d ^ t + 1 = d ^ t + f t θ w T θ ^ t + 1 = θ ^ t + w T

状态误差：

{d~t+1=dt+1−d^t+1θ~t+1=θt+1−θ^t+1 { d ~ t + 1 = d t + 1 − d ^ t + 1 θ ~ t + 1 = θ t + 1 − θ ^ t + 1

状态误差转移方程为：

[d~t+1θ~t+1]=[10∂fθt∂θ1][d~tθ~t] [ d ~ t + 1 θ ~ t + 1 ] = [ 1 ∂ f t θ ∂ θ 0 1 ] [ d ~ t θ ~ t ]

这里暂时不考虑噪声转移问题，将噪声视为加性噪声。
可得状态转移矩阵为：

Ft=[10∂fθt∂θ1] F t = [ 1 ∂ f t θ ∂ θ 0 1 ]

注意到里面对角线上面的是二阶导。
测量转移矩阵

Ht=[10] H t = [ 1 0 ]

演化噪声方差：

Qwt=diag[2000.2] Q t w = d i a g [ 2 0 0 0.2 ]

0.2是假设为 θ θ 的噪声方差。
测量噪声方差

Qwt=diag[3000.3] Q t w = d i a g [ 3 0 0 0.3 ]

0.3是假设为 θ θ 的测量噪声方差。
假设初始状态值为：

X0=[f0(0)0] X 0 = [ f 0 ( 0 ) 0 ]

.
误差协方差矩阵设定为;

P0=diag[2001] P 0 = d i a g [ 2 0 0 1 ]

matlab仿真代码如下：

clear
phi = 0;

y0 = 750;
x0 = 750;
R = 500;
T = 0.5;
v = 10;
r = 500;
w = v/r;
%f(phi) = sqrt((y0 - R*sin(phi))^2 + (x0 - R*cos(phi))^2)                  %真值
f =@(phi)250*((500*sin(phi) - 750)^2/62500 + (500*cos(phi) - 750)^2/62500)^(1/2);
%f_2_order_diff(phi) = w*T*diff(f(phi),2)
%                                                                                                                                                                                                                                                                                                                                                                                                                                                                
f_2_order_diff =@(phi)(5*(8*cos(phi)^2 + 8*sin(phi)^2 - (2*cos(phi)*(500*cos(phi) - 750))/125 - (2*sin(phi)*(500*sin(phi) - 750))/125))/(4*((500*sin(phi) - 750)^2/62500 + (500*cos(phi) - 750)^2/62500)^(1/2)) - (5*((2*cos(phi)*(500*sin(phi) - 750))/125 - (2*sin(phi)*(500*cos(phi) - 750))/125)^2)/(8*((500*sin(phi) - 750)^2/62500 + (500*cos(phi) - 750)^2/62500)^(3/2));

%过程噪声方差
Qw = diag([2 0.2]);
%测量噪声
Qv = diag([3 0.3]);
%状态值初始化
x_k1 = [f(phi) 0]';

%初始协方差
p_k1 = diag([2,1]);
%状态转移矩阵
F = [1 f_2_order_diff(phi);0 1];
%测量转移矩阵
H = [1 0;0 1];
%过程噪声
W_noise = diag([normrnd(0,2);normrnd(0,0.2)])*randn(2,4000);
%测量噪声
V_nosise = diag([normrnd(0,3);normrnd(0,0.1)])*randn(2,4000);
for i = 1:2000
    phi = phi + w*T;
    x_k2 = F*x_k1 + W_noise(:,i);                                      %下一时刻的先验状态预测   
    p_k2 = F*p_k1*F' + Qw;
    k = p_k2*H'*inv(H*p_k2*H' + Qv);   
    z_k1 =[f(phi)+wgn(1,1,10) 0]';    
    z_k = H*z_k1;
    Z_K(:,i) = z_k;    

    x_k1 = x_k2 + k*(z_k - H*x_k2);
    X_K(:,i) = x_k1;
    p_k1 = (eye(2) - k*H)*p_k2;    
end
figure
t = linspace(1,2000,2000);
plot(t,X_K(1,:),t,Z_K(1,:),'black');
legend('预测轨迹','真值轨迹');