EKF之雅克比矩阵（一）
| 阿里云国内75折 回扣 微信号：monov8 |
| 阿里云国际，腾讯云国际，低至75折。AWS 93折 免费开户实名账号 代冲值 优惠多多 微信号：monov8 飞机：@monov6 |
扩展卡尔曼滤波 (EKF)
文章目录
前言
一般的卡尔曼滤波KF是假设系统模型是线性的即系统的输入输出满足齐次性和叠加性。标准卡尔曼滤波的前提条件之一就是高斯分布的x_k 预测后仍服从高斯分布高斯分布的x_k 变换到观测空间后仍服从高斯分布因此标准卡尔曼滤波只适合线性系统的处理。
但是实际工程中卡尔曼滤波一般用于对传感器电机等电子器件。这些期间由于电子性质自身特性基本都是非线性的那么在这个系统之上建立的所有模型都符合非线性系统。
如何将一个非线性系统线性化就是EKF改进的部分。
一、什么是线性化
请大家看下面这个圆
一个平平无奇的圆其方程一般式如下
其方程式是一个明显的非线性函数看圆的形状也能看出来。图中也画出来了几个红色箭头指向的方向圆在某一点的切线方向。这个方向没有弯曲笔直的打出去那么在这个切线处圆是一条直线我们随意找一个切线然后放大
原谅我这个放大倍率已经是OneNote的极限的可以明显看到放大后的圆在切线处这一小段直线貌似是线性的。那么是否只要求出这个小小的线段的方程式我们就能把圆在这个点局部线性化了
这个方法直接告诉大家就是求圆在这个点处的导数即可得出这个小小的线段的方程式了。这就是一种局部线性化的方式。
而在卡尔曼滤波中一切数据传递计算更新都是通过矩阵来完成怎么把一个矩阵线局部线性化呢通过求该矩阵的雅克比矩阵即可。
二、雅克比矩阵
雅克比矩阵的概念我是从可汗学院看来的有兴趣大家可以移步b站看熟肉
可汗学院 雅克比矩阵
下面介绍下我印象比较深的几个点
1.矩阵的几何含义
已知一个矩阵 A =
[
1
0
0
1
]
(1)
\left[ \begin{matrix} 1 & 0 \\ 0 & 1 \\ \end{matrix} \right] \tag{1}
[1001](1)
他在笛卡尔坐标系下的几何含义为
一个小矩形沿x方向和沿y方向均有分量。
假设有另一个矩阵 B =
[
3
1
0
2
]
(2)
\left[ \begin{matrix} 3 & 1 \\ 0 & 2 \\ \end{matrix} \right] \tag{2}
[3012](2)
通过矩阵行列式可以算出该矩阵的标量其几何含义为向量[3,0]和向量[1,2]所围成的面积
我们将矩阵B做一个线性变化
del(B) = 3 * 2 - 1 * 0 = 6
如果把上面的矩阵 A 的面积为1如果我们把矩阵 A看做是基底那么矩阵B就是基底的6倍这样我们就可以把不同矩阵的几何意义联系在了一起。
但是我们能看出来矩阵A到矩阵B是一个很标准的线性关系那如果存在一组多元函数且该函数呈现非线性关系那么如何从几何层面理解这个多元函数与基底的关系呢
2.非线性矩阵与基底的关系
假设有一个矩阵C =
[ f 1 ( x , y ) f 2 ( x , y ) ] (3) \left[ \begin{matrix} f1(x,y) \\ f2(x,y) \\ \end{matrix} \right] \tag{3} [f1(x,y)f2(x,y)](3)
=
[ x + s i n ( y ) y + s i n ( x ) ] (4) \left[ \begin{matrix} x + sin(y) \\ y + sin(x) \\ \end{matrix} \right] \tag{4} [x+sin(y)y+sin(x)](4)
这个矩阵是个非常典型的非线性矩阵下面将坐标系中的所有x,y均乘以该矩阵即将该坐标系中的所有点均转换成矩阵C下的点抓住其中一个小矩形放大
这里怎么理解呢就是维持原有坐标系不变将坐标系中每个点通过矩阵C转移到新的地方矩阵C可以当成一个状态转移矩阵。转移后把每个点可视化出来即可得到上图扭曲的图形看起来和线性化根本不搭。
但是当对某一个区域放大时可以看到虽然新图形围城的小矩形不那么方正但是也没有变得特别扭曲这个小矩形和基底之间的关系就是其非线性化的程度。把这个区域再放大那么其形状越接近基底。这就说明新图形的整体是非线性的但是对局部而言仍可以看做是线性的。放大倍率越大其线性化程度越好。
(如上图经过局部放大后∂x默认为无穷小对x求偏导就可以把这个无穷小量化成正常的函数在x这个方向上的变化率。上图的基底是正交坐标系放大以后朝着x方向的伸缩能够正交分解成两个维度的变化率同理y方向也是一样的由此性质可以推出矩阵C的雅可比矩阵D)
3.雅克比矩阵
根据上面对圆的局部线性化的推导我们只要求出矩阵C导数就可以得出矩阵C的雅克比矩阵D了。一个矩阵是多维度的我们对这个矩阵的每一个维度求偏导如矩阵C存在二维x,y那么只需要求矩阵C对于x的偏导对于y的偏导即可推出雅克比矩阵。
CSDN打公式太麻烦了下面直接上图
三、工程中雅克比矩阵如何应用
卡尔曼滤波中需要对系统建模根据模型推导状态转移矩阵F和更新矩阵H通常还有个控制矩阵U不过这个不常用一般通过状态空间对系统建模太细就先不讲。
EKF中需要非线性化的就是矩阵F和矩阵H这两个矩阵分别用于协方差矩阵Pk的更新和卡尔曼系数Kk的更新。
前面说了矩阵有多个维度对卡尔曼这个系统而言每个状态向量X = [x1,x2,x3,x4, … , xn]都是一个维度那么就需要矩阵F对于矩阵X求偏导矩阵H对于矩阵X求偏导。
F(x1,x2,x3,x4) =
[ f 1 ( x 1 , x 2 , x 3 , x 4 ) f 2 ( x 1 , x 2 , x 3 , x 4 ) f 3 ( x 1 , x 2 , x 3 , x 4 ) f 4 ( x 1 , x 2 , x 3 , x 4 ) ] (5) \left[ \begin{matrix} f1(x1,x2,x3,x4) \\ f2(x1,x2,x3,x4) \\ f3(x1,x2,x3,x4) \\ f4(x1,x2,x3,x4) \\ \end{matrix} \right] \tag{5} ⎣⎢⎢⎡f1(x1,x2,x3,x4)f2(x1,x2,x3,x4)f3(x1,x2,x3,x4)f4(x1,x2,x3,x4)⎦⎥⎥⎤(5)
JF =
[
∂
f
1
/
∂
x
1
∂
f
1
/
∂
x
2
∂
f
1
/
∂
x
3
∂
f
1
/
∂
x
4
∂
f
2
/
∂
x
1
∂
f
2
/
∂
x
2
∂
f
2
/
∂
x
3
∂
f
2
/
∂
x
4
∂
f
3
/
∂
x
1
∂
f
3
/
∂
x
2
∂
f
3
/
∂
x
3
∂
f
3
/
∂
x
4
∂
f
4
/
∂
x
1
∂
f
4
/
∂
x
2
∂
f
4
/
∂
x
3
∂
f
4
/
∂
x
4
]
(6)
\left[ \begin{matrix} ∂f1/∂x1 & ∂f1/∂x2 & ∂f1/∂x3 & ∂f1/∂x4 \\ ∂f2/∂x1 & ∂f2/∂x2 & ∂f2/∂x3 & ∂f2/∂x4 \\ ∂f3/∂x1 & ∂f3/∂x2 & ∂f3/∂x3 & ∂f3/∂x4 \\ ∂f4/∂x1 & ∂f4/∂x2 & ∂f4/∂x3 & ∂f4/∂x4 \\ \end{matrix} \right] \tag{6}
⎣⎢⎢⎡∂f1/∂x1∂f2/∂x1∂f3/∂x1∂f4/∂x1∂f1/∂x2∂f2/∂x2∂f3/∂x2∂f4/∂x2∂f1/∂x3∂f2/∂x3∂f3/∂x3∂f4/∂x3∂f1/∂x4∂f2/∂x4∂f3/∂x4∂f4/∂x4⎦⎥⎥⎤(6)
总结
- EKF(扩展卡尔曼滤波)是KF(卡尔曼滤波)的扩展使其可以用于非线性系统在实际工程中很有用。
- EKF线性化非线性系统的方式是求矩阵的雅克比矩阵是一种局部线性化策略但仍会有转移误差。
- 另一种KF的改进UFK无迹卡尔曼通过对局部连续采样得出局部的信息用于卡尔曼滤波。只针对符合高斯分布的系统。
- PF粒子滤波也是对局部连续采样但并不要求系统符合高斯分布。
| 阿里云国内75折 回扣 微信号：monov8 |
| 阿里云国际，腾讯云国际，低至75折。AWS 93折 免费开户实名账号 代冲值 优惠多多 微信号：monov8 飞机：@monov6 |