卡尔曼滤波（Kalman Filtering）——（7）扩展卡尔曼滤波（EKF）一阶滤波_综合

格物致知之卡尔曼滤波（Kalman Filtering）——（7）扩展卡尔曼滤波（EKF）一阶滤波

一、状态空间模型
二、泰勒级数展开公式
三、麦克劳林展开公式
三、多维向量函数泰勒级数一维展开
五、一阶泰勒展式取近似
六、kalman与扩展kalman 公式对比
参考文献

??卡尔曼滤波器是为线性系统设计的算法，不能用于非线性系统中。但是事实上在实际工程中多数系统都是非线性的，所以如果卡尔曼滤波器不能用在非线性系统中的话，那么它的应用范围就非常有限了。扩展卡尔曼滤波（EKF）就是为了打破卡尔曼滤波器只能用于线性系统的局限性而设计出来的，一般解决非线性系统线性化是通过泰勒级数展开，省略高阶项得到一个近似的线性系统。这种处理非线性系统的方法被称为扩展卡尔曼滤波（EKF）。

一、状态空间模型

在这里插入图片描述

二、泰勒级数展开公式

??泰勒级数展开是将一个在 $x=x_{0}$ 处具有 $n$ 阶导数的函数 $f (x)$ , 利用关于 $(x?x0)\left(x-x_{0}\right)$ 的 $n$ 次多项式逼近函数值的方法。
??若函数 $f (x)$ 在包含 $x_{0}$ 的某个闭区间 $[a, b]$ 上具有 $n$ 阶导数，且在开区间 $(a, b)$ 上具有 $(n + 1)$ 阶导数，则对闭区间 $[a, b]$ 上的任意一点 $x$ ，都有：
$f(x)=f(x0)0!+f′(x0)1!(x?x0)+…+f(n)(x0)n!(x?x0)n+Rn(x)f(x)=\frac{f\left(x_{0}\right)}{0 !}+\frac{f^{\prime}\left(x_{0}\right)}{1 !}\left(x-x_{0}\right)+\ldots+\frac{f^{(n)}\left(x_{0}\right)}{n !}\left(x-x_{0}\right)^{n}+R_{n}(x)$
其中 $f(n)(x0)f^{(n)}\left(x_{0}\right)$ 表示函数 $f (x)$ 在 $x=x_{0}$ 处的 $n$ 阶导数，等式右边成为泰勒展开式，剩余的 $R_{n}(x)$ 是泰勒展开式的余项，是 $(x?x0)n\left(x-x_{0}\right)^{n}$ 的高阶无穷小。

三、麦克劳林展开公式

??函数的麦克劳林展开指上面泰勒公式中 $x_{0}$ 取0的情况，即是泰勒公式的特殊形式，若 $f (x)$ 在 $x = 0$ 处n阶连续可导，则下式成立：
$f(x)=f(0)+f′(0)1!x+f′′(0)2!x2+f′′′(0)3!x3+?+f(n)(0)n!xn+Rn(x)f(x)=f(0)+\frac{f^{\prime}(0)}{1 !} x+\frac{f^{\prime \prime}(0)}{2 !} x^{2}+\frac{f^{\prime \prime \prime}(0)}{3 !} x^{3}+\cdots+\frac{f^{(n)}(0)}{n !} x^{n}+R_{n}(x)$
其中 $f^{(n)}(x)$ 表示 $f (x)$ 的n阶导数。
麦克劳林展开的形象描述图：
在这里插入图片描述

??实际应用中，泰勒公式需要截断，只取有限项，一个函数的有限项的泰勒级数叫做泰勒展开式。泰勒公式的余项可以用于估算这种近似的误差。泰勒展开式的重要性体现在以下三个方面：

幂级数的求导和积分可以逐项进行，因此求和函数相对比较容易。
一个解析函数可被延伸为一个定义在复平面上的一个开片上的解析函数，并使得复分析这种手法可行。
泰勒级数可以用来近似计算函数的值。

三、多维向量函数泰勒级数一维展开

??当变量是多维向量时，一维的泰勒展开就需要做拓展，具体形式如下：
$f(x)=f(xk)+[?f(xk)]T(x?xk)+onf(\mathbf{x})=f\left(\mathbf{x}_{k}\right)+\left[\nabla f\left(\mathbf{x}_{k}\right)\right]^{T}\left(\mathbf{x}-\mathbf{x}_{k}\right)+o^{n}$
其中, $[?f(xk)]T=J(xk)\left[\nabla f\left(\mathbf{x}_{k}\right)\right]^{T}=\mathbf{J}\left(\mathbf{x}_{k}\right)$ 表示雅克比矩阵， $on\mathbf{o}^{n}$ 表示高阶无穷小。
$[?f(x)]T=J(x)=[?f(x)1?x1…?f(x)1?xn????f(x)m?x1…?f(x)m?xn][\nabla f(\mathbf{x})]^{T}=\mathbf{J}(\mathbf{x})=\left[\begin{array}{ccc} \frac{\partial f(\mathbf{x})_{1}}{\partial \mathbf{x}_{1}} & \ldots & \frac{\partial f(\mathbf{x})_{1}}{\partial \mathbf{x}_{n}} \\ \vdots & \ddots & \vdots \\ \frac{\partial f(\mathbf{x})_{m}}{\partial \mathbf{x}_{1}} & \ldots & \frac{\partial f(\mathbf{x})_{m}}{\partial \mathbf{x}_{n}} \end{array}\right]$
这里， $f(xk)f\left(\mathbf{x}_{k}\right)$ 为 $m$ 维， $xk\mathbf{x}_{k}$ 状态向量为 $n$ 维， $?2f(xk)?xm?xn=?f(xk)T?xm?f(xk)?xn\frac{\partial^{2} f\left(\mathbf{x}_{k}\right)}{\partial x_{m} \partial x_{n}}=\frac{\partial f\left(\mathbf{x}_{k}\right)^{T}}{\partial x_{m}} \frac{\partial f\left(\mathbf{x}_{k}\right)}{\partial x_{n}}$ 。
一般来说，EKF在对非线性函数做泰勒展开时，只取到一阶导和二阶导，而由于二阶导的计算复杂性，更多的实际应用只取到一阶导，同样也能有较好的结果。取一阶导时，状态转移方程和观测方程就近似为线性方程，高斯分布的变量经过线性变换之后仍然是高斯分布，这样就能够延用标准卡尔曼滤波的框架。

五、一阶泰勒展式取近似

非线性状态方程
${Xk=f(xk?1,uk?1,wk?1)Zk=h(xk,vk)wk?1?N(0,Q)，vk?1?N(0,P)\begin{aligned} \begin{cases} X_k=f(x_{k-1},u_{k-1},w_{k-1}) \\ Z_k=h(x_k,v_k) \end{cases} \\w_{k-1}\sim N(0,Q) ，v_{k-1}\sim N(0,P) \end{aligned}$
虽然系统有误差但是其误差任然服从正态分布，由于系统有误差，也不能在真实值处进行线性化，所以 $f({x}_{k})$ 在 $x^k?1\hat{x}_{k-1}$ （ $k ? 1$ 时刻的后验估计即上一次）处进行线性化操作。 ¹

用泰勒展开进行线性化操作：
$Xk=f(xk?1,uk?1,wk?1)+A(xk?x^k?1)+wwk?1\begin{aligned} &X_k=f(x_{k-1},u_{k-1},w_{k-1})+A(x_k-\hat{x}_{k-1})+ww_{k-1}\\ \end{aligned}$
假设误差 $w_{k-1}=0$ 则：
$Xk=f(xk?1,uk?1,0)+A(xk?x^k?1)+wwk?1A=?f?x∣x^k?1,uk?1w=?f?w∣x^k?1,uk?1\begin{aligned} \\&X_k=f(x_{k-1},u_{k-1},0)+A(x_k-\hat{x}_{k-1})+ww_{k-1} \\&A=\frac{\partial f}{\partial x}\mid_{ \hat{x}_{k-1},u_{k-1}} \\&w=\frac{\partial f}{\partial w}\mid_{ \hat{x}_{k-1},u_{k-1}} \end{aligned}$

令 $f(xk?1,uk?1,0)=x~kf(x_{k-1},u_{k-1},0)=\tilde{x}_k$ ，则 $Z_k$ 在 $x~k\tilde{x}_k$ 处线性化：
$Zk=h(x~k,vk)+H(xk?x~k)+vvk\begin{aligned} \\&Z_k=h(\tilde{x}_k,v_k)+H(x_k-\tilde{x}_k)+vv_k \end{aligned}$
假设误差 $v_{k}=0$ 则：
$Zk=h(x~k,0)+H(xk?x~k)+vvkH=?h?x∣x~kv=?h?v∣x~k\begin{aligned} \\&Z_k=h(\tilde{x}_k,0)+H(x_k-\tilde{x}_k)+vv_k \\&H=\frac{\partial h}{\partial x}\mid_{ \tilde{x}_{k}} \\&v=\frac{\partial h}{\partial v}\mid_{ \tilde{x}_{k}} \end{aligned}$
令 $h(x~k,0)=z~kh(\tilde{x}_k,0)=\tilde{z}_k$ ，则：
${Xk=x~k+A(xk?x^k?1)+wwk?1Zk=z~k+H(xk?x~k)+vvk\begin{aligned} \\\begin{cases} X_k=\tilde{x}_k+A(x_k-\hat{x}_{k-1})+ww_{k-1} \\Z_k=\tilde{z}_k+H(x_k-\tilde{x}_k)+vv_k \end{cases} \end{aligned}$

由于 $P（w）?N(0,Q)P（w）\sim N(0,Q)$ ，则：
${P（wwk?1）?N(0,WQWT)P（vvk）?N(0,VPVT)\begin{aligned} \begin{cases} P（ww_{k-1}）\sim N(0,WQW^T) \\P（vv_k）\sim N(0,VPV^T) \end{cases} \end{aligned}$

六、kalman与扩展kalman 公式对比

方法	kalman 公式	扩展kalman 公式
先验估计	$x^k?=Ax^k?1+Buk?1\hat{x}_{k}^-=A \hat{x}_{k-1}+B u_{k-1}$	$X^k?=f(xk?1,uk?1,0)\hat{X}_k^-=f(x_{k-1},u_{k-1},0)$
先验误差协方差矩阵	$P_{k}^{-}=A P_{k-1} A^{T}+Q$	$P_{k}^{-}=A P_{k-1} A^{T}+WQW^T$
卡尔曼增益	$Kk=Pk?HTHPk?HT+RK_{k}=\frac{P_{k}^{-} H^{T}}{H P_{k}^{-} H^{T}+R}$	$Kk=Pk?HTHPk?HT+VRVTK_{k}=\frac{P_{k}^{-} H^{T}}{H P_{k}^{-} H^{T}+VRV^T}$
后验估计	$x^k=x^k?+Kk(zk?Hx^k?)\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}\left(z_{k}-H \hat{x}_{k}^{-}\right)$	$x^k=x^k?+Kk(zk?h(x~k,0))\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}\left(z_{k}-h(\tilde{x}_k,0)\right)$
更新误差协方差矩阵	$Pk=(I?KkH)Pk?P_{k}=\left(I-K_{k} H\right) P_{k}^{-}$	$Pk=(I?KkH)Pk?P_{k}=\left(I-K_{k} H\right) P_{k}^{-}$

参考文献

【卡尔曼滤波器】6_扩展卡尔曼滤波器_Extended Kalman Filter
线性化

在扩展卡尔曼滤波中，我们并不用前一个时刻的先验值 $X_k^－$ （卡尔曼滤波器未经过修正的预测值）作为参考点，而是用前一个时刻的估计值作为参考点做线性化。这是因为相对于先验值，前一个时刻的估计值更加贴近于真实值，将估计值作为线性化参考点可以得到一个更加贴近于实际的线性化系统模型。 ??

卡尔曼滤波（Kalman Filtering）——（7）扩展卡尔曼滤波（EKF）一阶滤波