卡尔曼滤波器：6.扩展卡尔曼滤波器

卡尔曼滤波器：6.扩展卡尔曼滤波器

卡尔曼滤波器是线性系统下最优的滤波器

这部分呢，讲一下，非线性环境下 扩展卡尔曼滤波器

对于非线性环境下的处理呢 有很多种处理方法，这部分就是讲最基础的方法，扩展卡尔曼滤波器的方式

非线性模型：

KaTeX parse error: No such environment: flalign* at position 8: \begin{̲f̲l̲a̲l̲i̲g̲n̲*̲}̲ &x_k=f(x_{k-1}…

正太分布分随机变量通过非线性系统后就不再是正态的了

如果我们还想向这个系统使用卡尔曼滤波的话，就是要对该系统实现线性化

线性化 linerization

线性化会利用到泰勒展开（taylor series）
$$f(x)=f(x_0)+\frac{\delta f}{\delta x}(x-x_0)$$
对于高纬度的来讲，就要用到雅可比矩阵（在非线性系统中有很详细的讲解）

最好的线性化的点 真实值 但是系统有误差，无法在真实点线性化

估计方程线性化

$f(x_k)$在${\hat{x}}_{k-1}$附近线性化即k-1时刻的后验估计

因此线性化模型可以变成这样：
$$x_k=f({\hat{x}}_{k-1},{\mu }_{k-1},{\omega }_{k-1})+A(x_k-{\hat{x}}_{k-1})+{\omega }_{{\omega }_{k-1}}$$
误差为0，

$f({\hat{x}}_{k-1},{\mu }_{k-1},0)={\tilde{x}}_k$

A是雅可比矩阵
KaTeX parse error: No such environment: flalign* at position 8: \begin{̲f̲l̲a̲l̲i̲g̲n̲*̲}̲ &A=\frac{\delt…
然后把${\hat{x}}_{k-1},{\mu }_{k-1}$代入进去就可以了

为了方便理解，举个栗子：（二维系统）
$$\begin{gathered} x_1=x_1+sin(x_2)=f_1 \\ {x}_2=x_1^2=f_2 \end{gathered}$$

$$
\begin{align}

A&=\frac{\delta{f}}{\delta{x}}\
&=\left[\matrix{\frac{\delta{f_1}}{\delta{x_1}}&\frac{\delta{f_1}}{\delta{x_2}}\
\frac{\delta{f_2}}{\delta{x_1}}&\frac{\delta{f_2}}{\delta{x_2}}}\right]\
&=\left[\matrix{1&cos(x_2)\2x_1&0}\right]{|\hat{x}{1,k-1}, \hat{x}{2,k-1}}\
&=\left[\matrix{1&cos(\hat{x}{2,k-1})\2\hat{x}_{1,k-1}&0}\right]
\end{align}
$$

这样就得到了一个A矩阵在K时刻的样子

因为A矩阵在随着k的变换而变化，因此我们每一个时刻K矩阵都在变化中每次都需要对该矩阵进行求解
KaTeX parse error: Undefined control sequence: \matrix at position 12: A_k=\left[\̲m̲a̲t̲r̲i̲x̲{1&cos(\hat{x}_…
同样的可以求解出来

${\omega }_k=\frac{\delta f}{\delta \omega }$

然后把${\hat{x}}_{k-1},{\mu }_{k-1}$代入进去就可以了

测量方程线性化

$z_k$在${\tilde{x}}_k$线性化
$$z_k=h({\tilde{x}}_k，{\nu }_k)+H(x_k-{\tilde{x}}_k)+{\nu }_{{\nu }_k}$$
$h({\tilde{x}}_{k-1},0)={\tilde{z}}_k$

$H={\frac{\delta h}{\delta x}}_{|{\tilde{x}}_k}$

$\nu ={\frac{\delta h}{\delta \nu }}_{|{\tilde{x}}_k}$

综合

$$\begin{gathered} x_k={\tilde{x}}_k+A(x_{k-1}-{\hat{x}}_{k-1})+{\omega }_{{\omega }_{k-1}} \\ {z}_k={\tilde{z}}_k+H(x_k-{\tilde{x}}_k)+{\nu }_{{\nu }_k} \end{gathered}$$

其中
$$\begin{gathered} P(\omega )\sim N(0,Q) \\ P({\omega }_{\omega })\sim N(0,\omega Q{\omega }^T) \\ P({\nu }_{\nu })\sim N(0,\nu R{\nu }^T) \end{gathered}$$
后面就可以用之前的方式进行推导了

公式

	预测		更新
先验	${\hat{x}}_k^{-}=f({\hat{x}}_{k-1},{\mu }_{k-1},0)$	卡尔曼增益	$K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+\nu R{\nu }^T}$
先验误差协方差	$P_k^{-}=A{P}_{k-1}{A}^T+\omega Q{\omega }^T$	后验估计	${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-h({\hat{x}}_k^{-},0))$
		更新误差协方差	$P_k=(I-K_{k}H)P_k^{-}$