做电影网站服务器,简易php企业网站源码,龙华网站(建设龙华信科),运城做网站要多少钱目录 前言数学理论推导1. 直观理解与二维实例2. EKF3. 补充知识-线性化结语参考 前言 最近项目需求涉及到目标跟踪部分#xff0c;准备从 DeepSORT 多目标跟踪算法入手。DeepSORT 中涉及的内容有点多#xff0c;以前也就对其进行了简单的了解#xff0c;但是真正去做发现总是… 目录 前言数学理论推导1. 直观理解与二维实例2. EKF3. 补充知识-线性化结语参考 前言 最近项目需求涉及到目标跟踪部分准备从 DeepSORT 多目标跟踪算法入手。DeepSORT 中涉及的内容有点多以前也就对其进行了简单的了解但是真正去做发现总是存在这样或者那样的困惑头疼果然欠下的总该还的 一个个来吧这个系列文章主要分享博主在学习 DeepSORT 中的 Kalman Filter 的相关知识主要从两方面分享一方面是数学理论推导另一方面是比较通俗易懂的图例分析。 这篇文章主要分享从数学理论推导的方式去理解卡尔曼滤波器包括卡尔曼滤波器应用的实例和扩展卡尔曼滤波 博主为初学者欢迎交流讨论若有问题欢迎各位看官批评指正 数学理论推导 视频链接【卡尔曼滤波器】_Kalman_Filter_全网最详细数学推导 注博主也就把 DR_CAN 老师讲解的内容复述了一遍强烈建议大家观看原视频 1. 直观理解与二维实例
在前面的内容中我们详细推导了卡尔曼滤波器的五个公式和递归算法的概念在这节内容我们将通过一个二维的例子来给大家分析一下卡尔曼滤波器再来直观的感受一下它的魅力。
DR_CAN 老师是用 Excel 做了一个完整的二维系统的卡尔曼滤波的示例里面的公式都是用 Excel 做的下载链接Baidu Driver【提取码txn3】
其中的文档做了加密处理密码是下面一个简单的数据融合的应用
如果我们用两个尺子去测量一个物体得到两个数据分别是 Z 1 6.5 m m Z_16.5mm Z16.5mm Z 2 7.3 m m Z_27.3mm Z27.3mm又已知两个尺子的标准差分别为 σ 1 0.2 m m \color{blue}\sigma_10.2mm σ10.2mm σ 2 0.4 m m \color{blue}\sigma_20.4mm σ20.4mm这个时候请问最优的估计值 Z ^ \hat{Z} Z^ 是多少最优估计值 Z ^ \hat{Z} Z^ 即为文档的密码。 那我们一起来看下这个例子这是一个很简单的应用一个人在走路对于这个系统来说可以设定两个状态变量分别是 X 1 X_1 X1 代表了位置 X 2 X_2 X2 代表了速度。
如果我们假设它在匀速行驶那么它的这种离散形式的状态空间方程如下
速度 X 2 , k X 2 , k − 1 X_{2,k} X_{2,k-1} X2,kX2,k−1这里面采样时间定义为 Δ T \Delta T ΔT 代表 k k k 时刻与 k − 1 k-1 k−1 时刻之间的时间间隔。
位置 X 1 , k X 1 , k − 1 Δ T X 2 , k − 1 X_{1,k} X_{1,k-1}\Delta{T}X_{2,k-1} X1,kX1,k−1ΔTX2,k−1也就是 k k k 时刻的位置等于 k − 1 k-1 k−1 时刻时的位置加上速度乘以时间间隔
这里为了简化处理我们令采样时间 Δ T 1 \Delta{T}1 ΔT1可以看出来这两个方程是在理想状态下的而在现实生活中存在着很多的不确定性有一些误差。比如这个路上有上坡也有下坡然后走的时候也有时慢有时快很难保持匀速。所以说这个数学模型就要加上一些不确定性如下 X 1 , k X 1 , k − 1 X 2 , k − 1 W 1 , k − 1 X 2 , k X 2 , k − 1 s X 2 , k − 1 W 2 , k − 1 \begin{aligned} X_{1,k} X_{1,k-1} X_{2,k-1} W_{1,k-1} \\ X_{2,k} X_{2,k-1} \hphantom{sX_{2,k-1}}W_{2,k-1} \end{aligned} X1,kX2,kX1,k−1X2,k−1W1,k−1X2,k−1sX2,k−1W2,k−1 这里面 W W W 叫做过程噪声这两个都是不确定的但是在卡尔曼滤波分析里面我们假设它们都是符合自然界最广泛的正态分布的它们的期望是 0 0 0协方差矩阵是 Q Q Q即 P ( W ) ∼ N ( 0 , Q ) P(W) \sim N(0,Q) P(W)∼N(0,Q)
所以说在这种情况下我们就无法通过这两个方程来准确地得到 k k k 时刻的状态变量 X 1 X_1 X1 和 X 2 X_2 X2 了但是我们可以估计根据这个误差的噪声来估计。
同时除了这个数学模型我们又在天上放了一个卫星来时刻监测这个人的位置和距离这样的话就得到了一个测量方程 Z 1 , k X 1 , k Z 2 , k X 2 , k \begin{aligned} \color{blue}Z_{1,k} X_{1,k} \\ \color{blue}Z_{2,k} X_{2,k} \end{aligned} Z1,kZ2,kX1,kX2,k 卫星测量位置和速度两个内容如果说这两个测量准确的话那就可以精确的知道在 k k k 时刻这个人的位置和速度也就用不上什么滤波器了。
但是在现实生活当中都是存在着不确定性的在测量的过程当中可能会测到不同的物体而且这个传感器本身这个卫星本身它也有一定的误差所以这里面也要加上一定的不确定性如下 Z 1 , k X 1 , k V 1 , k Z 2 , k X 2 , k V 2 , k \begin{aligned} \color{blue}Z_{1,k} X_{1,k} V_{1,k}\\ \color{blue}Z_{2,k} X_{2,k} V_{2,k} \end{aligned} Z1,kZ2,kX1,kV1,kX2,kV2,k 这里面 V V V 叫做测量误差这两个都是不确定的它们也符合正态分布期望是 0 0 0协方差矩阵是 R R R即 P ( V ) ∼ N ( 0 , R ) P(V) \sim N(0,R) P(V)∼N(0,R)
下面我们可以把这两个式子用比较紧凑的矩阵的方式来表达出来它就是下面的形式 [ X 1 , k X 2 , k ] [ 1 1 0 1 ] [ X 1 , k − 1 X 2 , k − 1 ] [ W 1 , k − 1 W 2 , k − 1 ] [ Z 1 , k Z 2 , k ] [ 1 0 0 1 ] [ X 1 , k X 2 , k ] [ V 1 , k V 2 , k ] \begin{aligned} \begin{bmatrix} X_{1,k} \\ X_{2,k} \end{bmatrix}\phantom{} \begin{bmatrix} 1 1 \\ 0 1 \end{bmatrix} \begin{bmatrix} X_{1,k-1} \\ X_{2,k-1} \end{bmatrix} \begin{bmatrix} W_{1,k-1} \\ W_{2,k-1} \end{bmatrix}\\ \begin{bmatrix} Z_{1,k} \\ Z_{2,k} \end{bmatrix}\phantom{} \begin{bmatrix} 1 0 \\ 0 1 \end{bmatrix} \begin{bmatrix} X_{1,k} \\ X_{2,k} \end{bmatrix} \begin{bmatrix} V_{1,k} \\ V_{2,k} \end{bmatrix} \end{aligned} [X1,kX2,k][Z1,kZ2,k][1011][X1,k−1X2,k−1][W1,k−1W2,k−1][1001][X1,kX2,k][V1,kV2,k] 然后这个时候如果我们把它归纳一下它就变成了如下的形式 X k A X k − 1 W k − 1 Z k H X k V k \begin{aligned} \color{red} X_k \phantom{} \color{red} AX_{k-1}W_{k-1} \\ \color{red}Z_k \phantom{} \color{red} HX_k V_k \end{aligned} XkZkAXk−1Wk−1HXkVk 这样一来如果去估计 k k k 时刻时 X 1 , k X_{1,k} X1,k 和 X 2 , k X_{2,k} X2,k 的值想得到一个相对准确的结果的话这就是一个数据融合的概念。通过一个不太准确的数学模型和一个不太准确的测量结果去估计一个最优值而这个最优值估计的算法就是卡尔曼滤波器。
说它是最优算法是因为它在数学上得到了证明大家可以去回看前几篇文章的理论推导。而卡尔曼滤波器分为两个步骤第一个步骤是预测第二个步骤是校正
预测 X ^ k − A X ^ k − 1 − B U k − 1 P k − A P k − 1 A T Q \begin{aligned} \hat{X}_k^- A\hat{X}_{k-1}^- BU_{k-1} \\ P_k^- AP_{k-1}A^T Q \\ \end{aligned} X^k−Pk−AX^k−1−BUk−1APk−1ATQ 校正 K k P k − H T H P k − H T R X ^ k X ^ k − K k ( Z k − H X ^ k − ) P k ( I − K k H ) P k − \begin{aligned} K_k \frac{P_k^-H^T}{HP_k^-H^TR} \\ \hat{X}_k \color{black} \hat{X}_k^-K_k(Z_k-H\hat{X}_k^-) \\ P_k (I-K_kH)P_k^- \end{aligned} KkX^kPkHPk−HTRPk−HTX^k−Kk(Zk−HX^k−)(I−KkH)Pk− 通过上面这五个公式来完成这五个公式在我们之前的文章中已经详细讲过了这边就不再赘述了。
我们直接来看这个例子由于 DR_CAN 老师的 Excel 程序不方便展示这边博主参考了另一位同学的 Python 实现参考链接为https://github.com/liuchangji/2D-Kalman-Filter-Example_Dr_CAN_in_python
完整的代码如下
import numpy as np
import matplotlib.pyplot as pltdef gaussian_distribution_generator(var):return np.random.normal(loc0.0, scalevar, sizeNone)# 状态转移矩阵上一时刻的状态转移到当前时刻
A np.array([[1, 1],[0, 1]])# 过程噪声协方差矩阵Qp(w)~N(0,Q)噪声来自真实世界中的不确定性
Q np.array([[1, 0],[0, 1]])# 观测噪声协方差矩阵Rp(v)~N(0,R)
R np.array([[0.1, 0],[0, 0.1]])# 状态观测矩阵
H np.array([[1, 0],[0, 1]])# 控制输入矩阵B
B None
# 初始位置与速度
X0 np.array([[0],[1]])# 状态估计协方差矩阵P初始化
P np.array([[1, 0],[0, 1]])if __name__ __main__:# ---------------------------初始化-------------------------X_true np.array(X0) # 真实状态初始化X_posterior np.array(X0)P_posterior np.array(P)speed_true []position_true []speed_measure []position_measure []speed_prior_est []position_prior_est []speed_posterior_est []position_posterior_est []for i in range(30):# -----------------------生成真实值----------------------# 生成过程噪声w np.array([[gaussian_distribution_generator(Q[0, 0])],[gaussian_distribution_generator(Q[1, 1])]])X_true np.dot(A, X_true) w # 得到当前时刻状态speed_true.append(X_true[1, 0])position_true.append(X_true[0, 0])# -----------------------生成观测值----------------------# 生成观测噪声v np.array([[gaussian_distribution_generator(R[0, 0])],[gaussian_distribution_generator(R[1, 1])]])Z_measure np.dot(H, X_true) v # 生成观测值,H为单位阵Eposition_measure.append(Z_measure[0, 0])speed_measure.append(Z_measure[1, 0])# ----------------------进行先验估计---------------------X_prior np.dot(A, X_posterior)position_prior_est.append(X_prior[0, 0])speed_prior_est.append(X_prior[1, 0])# 计算状态估计协方差矩阵PP_prior_1 np.dot(A, P_posterior)P_prior np.dot(P_prior_1, A.T) Q# ----------------------计算卡尔曼增益,用numpy一步一步计算Prior and posteriork1 np.dot(P_prior, H.T)k2 np.dot(np.dot(H, P_prior), H.T) RK np.dot(k1, np.linalg.inv(k2))# ---------------------后验估计------------X_posterior_1 Z_measure - np.dot(H, X_prior)X_posterior X_prior np.dot(K, X_posterior_1)position_posterior_est.append(X_posterior[0, 0])speed_posterior_est.append(X_posterior[1, 0])# 更新状态估计协方差矩阵PP_posterior_1 np.eye(2) - np.dot(K, H)P_posterior np.dot(P_posterior_1, P_prior)# 可视化显示if True:fig, axs plt.subplots(1,2)axs[0].plot(speed_true, -, labelspeed_true, linewidth2) # Plot some data on the axes.axs[0].plot(speed_measure, -, labelspeed_measure, linewidth2) # Plot some data on the axes.axs[0].plot(speed_prior_est, -, labelspeed_prior_est, linewidth2) # Plot some data on the axes.axs[0].plot(speed_posterior_est, -, labelspeed_posterior_est, linewidth2) # Plot some data on the axes.axs[0].set_title(speed)axs[0].set_xlabel(k) # Add an x-label to the axes.axs[0].legend() # Add a legend.axs[1].plot(position_true, -, labelposition_true, linewidth1) # Plot some data on the axes.axs[1].plot(position_measure, -, labelposition_measure, linewidth1) # Plot some data on the axes.axs[1].plot(position_prior_est, -, labelposition_prior_est, linewidth1) # Plot some data on the axes.axs[1].plot(position_posterior_est, -, labelposition_posterior_est, linewidth1) # Plot some data on the axes.axs[1].set_title(position)axs[1].set_xlabel(k) # Add an x-label to the axes.axs[1].legend() # Add a legend.plt.show()代码的注释已经非常非常详细了大家自己多调试调试就明白了可以对照下面的公式来看 上面第一部分的公式是用来生成实际的位置和实际的速度即生成真实值这里面有一个点需要注意这两个误差 w w w 和 v v v 都是随机数通过 np.random.normal 语句生成。然后这里面我们做了一个简化也就是说误差项是相互独立的即 Q Q Q 矩阵和 R R R 矩阵只有对角线上有值非对角线值为 0说明它们之间的协方差为 0只有方差这一项没有协方差这一项。
上面第二部分的公式是利用卡尔曼滤波器来进行计算的包括预测和校正两部分。
代码中我们让过程协方差矩阵 Q Q Q 的值为 1测量协方差矩阵 R R R 的值为 0.1运行效果如下 上面的图是速度的比较下面的图是位置的比较。其中蓝色的直线 — \color{blue}\boldsymbol{—} — 表示真实值橙色的点 ∙ \color{orange}\bullet ∙ 表示测量值绿色的虚线 − − \color{green}\boldsymbol{--} −− 表示先验估计值红色的直线 — \color{red}\mathbf— — 表示后验估计值。
从上图你可以非常明显的看到红色的直线 — \color{red}\mathbf— — 和橙色的点 ∙ \color{orange}\bullet ∙ 非常的靠近因为我们说过它的测量协方差要小也就是测量的结果更加准确所以我们的后验结果当然就更靠近于测量结果也可以看到这个测量的结果都是非常靠近于这个实际的结果。
我们在这个地方变换一下我们让过程误差减小变为 0.1让测量误差增加变为 1再次运行效果如下 这时候你会发现测量值也就是橙色的点 ∙ \color{orange}\bullet ∙ 会离实际值即蓝色的直线 — \color{blue}\boldsymbol{—} — 非常的远因为它的测量误差很大但是当经过这个卡尔曼滤波器以后我们发现这个后验估计值并没有因为测量偏差而走得非常远反而是由于我们有这个先验计算的结果会把后验值拉得距离真实值非常的近使得估计值不会特别的离谱。
大家可以自己多去调整下各种参数的值来跑几个不同的数据来体会一下分析一下这个是最好的去掌握卡尔曼滤波器的方法了这就需要各位看官自己花一些时间了。 那在卡尔曼滤波器的实际应用中有哪些参数是需要我们去调整的呢即卡尔曼滤波器的调参是调整那些值 卡尔曼滤波器的调参是一个重要的过程涉及到很多参数。在实际应用中以下是经常需要调整的参数(from chatGPT)
1. 过程噪声协方差矩阵 Q Q Q这个矩阵表示系统的内部噪声也就是数学模型的不确定性。通常如果你认为你的模型有更多的不确定性或噪声你可以增加 Q Q Q 的值。反之如果你认为模型很稳定可以减少 Q Q Q 的值。
2. 观测噪声协方差矩阵 R R R这个矩阵表示观测值的噪声或不确定性。如果你认为测量传感器非常准确可以减少 R R R 的值。反之如果你认为传感器有很大的误差可以增加 R R R 的值。
3. 初始状态估计协方差矩阵 P P P这个矩阵表示对初始状态估计的不确定性。通常如果对初始状态非常有自信可以设置 P P P 为较小的值反之如果对初始状态不确定可以设置较大的值。
4. 初始状态值 X 0 X_0 X0根据问题和已知的先验知识选择。
调整这些参数的一般策略如下
模拟和真实数据比较: 如果有可用的真实数据可以使用它来调整参数使滤波器的输出尽可能接近真实数据。敏感性分析: 对于每个参数可以在一定范围内变化它然后观察输出的变化以了解哪些参数对结果最敏感。经验和直觉: 有时根据问题的物理背景和直觉可以对参数进行合理的估计。优化方法: 有些情况下可以使用优化算法自动调整参数以最小化某个误差指标。 Q Q Q 和 R R R 是两个关键参数它们之间的平衡对估计的质量至关重要。如果数学模型被认为是可靠的可以减小 Q Q Q 的值如果测量被认为是可靠的可以减小 R R R 的值。但过度信任其中一个可能导致估计结果的质量下降。 P P P 主要影响初始化几个时间步的估计。如果对初始状态非常有信心可以较小 P P P 的值。否则可以增大它允许估计在开始时有更大的波动随后逐渐稳定。
在实际应用中卡尔曼滤波器的参数调整通常需要多次迭代和实验以找到最佳的参数组合。
2. EKF
前面的文章中我们介绍了卡尔曼滤波器也通过了数学推导证明了卡尔曼滤波器是最优化的线性滤波器它用在线性系统里面可以得到最优的的估计值
这节内容我们介绍下卡尔曼滤波器在非线性系统中的应用对于非线性系统来说去处理它有非常多的方法有的时候需要具体问题去具体分析这节我们会介绍一种最基础的方法将非线性系统线性化这种滤波器就称为扩展卡尔曼滤波器Extended Kalman FilterEKF
首先我们先来复习一下线性系统现在有一个线性系统它的状态空间表达式如下 X k A X k − 1 B U k − 1 W k − 1 Z k H k − 1 V k \begin{aligned} X_k AX_{k-1}BU_{k-1}W_{k-1} \\ Z_k H_{k-1} V_k \end{aligned} XkZkAXk−1BUk−1Wk−1Hk−1Vk 这里面 W W W 是过程误差符合正态分布期望是 0 0 0协方差矩阵是 Q Q Q即 P ( W ) ∼ N ( 0 , Q ) P(W) \sim N(0,Q) P(W)∼N(0,Q) V V V 是测量误差也符合正态分布期望是 0 0 0协方差矩阵是 R R R即 P ( V ) ∼ N ( 0 , R ) P(V) \sim N(0,R) P(V)∼N(0,R) 在前面的文章中我们推导出了卡尔曼滤波器的五个公式它的表达形式如下所示包含预测部分和校正部分
预测 X ^ k − A X ^ k − 1 B U k − 1 P k − A P k − 1 A T Q \begin{aligned} \hat{X}_k^- A\hat{X}_{k-1} BU_{k-1} \\ P_k^- AP_{k-1}A^T Q \end{aligned} X^k−Pk−AX^k−1BUk−1APk−1ATQ 校正 K k P k − H T H P k − H T R X ^ k X ^ k − K k ( Z k − H X ^ k − ) P k ( I − K k H ) P k − \begin{aligned} K_k \frac{P_k^-H^T}{HP_k^-H^TR} \\ \hat{X}_k \hat{X}_k^- K_k(Z_k - H\hat{X}_k^-) \\ P_k (I - K_kH)P_k^- \end{aligned} KkX^kPkHPk−HTRPk−HTX^k−Kk(Zk−HX^k−)(I−KkH)Pk− 预测部分先来计算它的先验 X ^ k − \color{blue}\hat{X}_k^- X^k−然后校正部分得到它的后验估计值 X ^ k \color{blue}\hat{X}_k X^k这是一个数据融合 Data Fusion 的一个思想在这里。 下面我们来考虑非线性系统对于一个非线性系统来说它没办法用线性的状态空间方程来表达而是用下面的这种形式 X k f ( X k − 1 , U k − 1 , W k − 1 ) Z k h ( X k , V k ) \begin{aligned} X_k f(X_{k-1},U_{k-1},W_{k-1}) \\ Z_k h(X_k,V_k) \end{aligned} XkZkf(Xk−1,Uk−1,Wk−1)h(Xk,Vk) 这里面 f f f 和 h h h 是两个非线性的表达形式大家要注意不管它是线性还是非线性的这里面所有的误差它都是符合正态分布的否则的话我们就没有办法操作了
这里面存在着一个问题那就是正态分布的随机变量通过非线性系统后就不再是正态分布的了
就比如说一个正态分布通过一条蓝色的直线它是线性的通过它映射到另一个平面上它依然会是一个正态分布的图如下图所示 但是如果说它通过一个非线性的映射比如说下面红色的曲线它所表现出来的就不再会是正态分布的一种情况了如下图所示 所以如果我们还想对于这个系统使用卡尔曼滤波的话就需要对它进行线性化 Linearization线性化会用到泰勒级数简单来说就是 f ( x ) f ( x 0 ) ∂ f ∂ x ( x − x 0 ) f(x) f(x_0) \frac{\partial{f}}{\partial{x}}(x-x_0) f(x)f(x0)∂x∂f(x−x0) 如果我们用一个一维的例子来看这样一个非线性的曲线我们要去预测 f ( x ) f(x) f(x) 可以使用下面的方式 f ( x ) f ( x 0 ) k ( x − x 0 ) f(x) f(x_0) k(x-x_0) f(x)f(x0)k(x−x0) 这是一个简单的三角的几何关系而对于高纬度来讲它就要用到雅可比矩阵关于这部分内容可以参考【工程数学基础】2_线性化_泰勒级数_泰勒公式_Linearization
可以看得出来我们如果想去线性化一个系统我们需要找到一个点比如上面的 x 0 x_0 x0它被称为 Operating Point然后在它附近进行线性化
对于我们这个非线性的系统来说最好的一个线性化的点当然就是它的真实点但是这里面有问题因为系统有误差我们永远没办法知道真实点是多少这个真实值是多少所以只能退而求其次对于这个过程方程来说我们让 f ( x k ) f(x_k) f(xk) 在 x ^ k − 1 \hat{x}_{k-1} x^k−1 处进行线性化也就是 k − 1 k-1 k−1 时刻的后验估计我们在这个附近把它进行线性化因此方程可以写作 X k f ( X ^ k − 1 , U k − 1 , W k − 1 ) A ( X k − X ^ k − 1 ) W w k − 1 X_k f(\hat{X}_{k-1},U_{k-1},W_{k-1}) A(X_k - \hat{X}_{k-1}) Ww_{k-1} Xkf(X^k−1,Uk−1,Wk−1)A(Xk−X^k−1)Wwk−1 这里面 W k − 1 W_{k-1} Wk−1 是误差我们并不知道它是多少所以我们简单的把它假设为 0进一步简化我们令 f ( X ^ k − 1 , U k − 1 , 0 ) X ~ k f(\hat{X}_{k-1},U_{k-1},0) \widetilde{X}_k f(X^k−1,Uk−1,0)X k
其中 A A A 是雅可比矩阵 A ∂ f ∂ X ∣ X ^ k − 1 , U k − 1 A \frac{\partial{f}}{\partial{X}}{\vert_{\hat{X}_{k-1},U_{k-1}}} A∂X∂f∣X^k−1,Uk−1
举一个二维的例子比如说有一个二维的系统如下 x 1 x 1 sin x 2 f 1 x 2 x 1 2 f 2 A ∂ f ∂ x [ ∂ f 1 ∂ x 1 ∂ f 1 ∂ x 2 ∂ f 2 ∂ x 1 ∂ f 2 ∂ x 2 ] [ 1 cos x 2 2 x 1 0 ] ∣ x ^ 2 , k − 1 x ^ 1 , k − 1 A k [ 1 cos x 2 , k − 1 2 x 1 , k − 1 0 ] \begin{aligned} x_1 x_1 \sin{x_2} f_1 \\ x_2 x_1^2 f_2 \\ A \frac{\partial{f}}{\partial{x}} \begin{bmatrix}\frac{\partial{f}_1}{\partial{x}_1} \frac{\partial{f}_1}{\partial{x}_2}\\\frac{\partial{f}_2}{\partial{x}_1} \frac{\partial{f}_2}{\partial{x}_2}\end{bmatrix} \\ \begin{bmatrix}1 \cos{x_2}\\2x_1 0\end{bmatrix}_{\vert_{\hat{x}_{2,k-1}}^{\hat{x}_{1,{k-1}}}}\\ A_k \begin{bmatrix}1 \cos{x_{2,k-1}}\\2x_{1,k-1} 0\end{bmatrix} \end{aligned} x1x2AAkx1sinx2f1x12f2∂x∂f[∂x1∂f1∂x1∂f2∂x2∂f1∂x2∂f2][12x1cosx20]∣x^2,k−1x^1,k−1[12x1,k−1cosx2,k−10] 从上式可以看出 A A A 矩阵随着 k k k 的变化在不断变化也就是说我们每一步都要重新去计算 A A A 矩阵它是不断变换的这个线性化的结果是在不断变化的
同理 W k ∂ f ∂ W ∣ X ^ k − 1 , U k − 1 W_k \frac{\partial{f}}{\partial{W}}{\vert_{\hat{X}_{k-1},U_{k-1}}} Wk∂W∂f∣X^k−1,Uk−1
这是关于过程方程的线性化对于测量方程来说 Z k Z_k Zk 我们把它在 X ~ k \widetilde{X}_k X k 进行线性化因此方程可以写为 Z k h ( X ~ k , V k ) H ( X k − X ~ k ) V v k Z_k h(\widetilde{X}_k,V_k) H(X_k - \widetilde{X}_k) Vv_k Zkh(X k,Vk)H(Xk−X k)Vvk 和上面的处理一样由于我们没办法得到 V k V_k Vk 的结果假设它是 0因此我们也可以令 h ( X ~ k , 0 ) Z ~ k \color{blue}h(\widetilde{X}_k,0)\widetilde{Z}_k h(X k,0)Z k H ∂ h ∂ x ∣ X ~ k H \frac{\partial{h}}{\partial{x}}\vert_{\widetilde{X}_k} H∂x∂h∣X k V ∂ h ∂ v ∣ X ~ k V \frac{\partial{h}}{\partial{v}}\vert_{\widetilde{X}_k} V∂v∂h∣X k
我们可以推导出下式 X k X ~ k A ( X k − 1 − X ^ k − 1 ) W w k − 1 Z k Z ~ k H ( X k − X ~ k ) V v k \begin{aligned} \color{red}X_k \color{black}\widetilde{X}_k A(X_{k-1}-\hat{X}_{k-1})Ww_{k-1} \\ \color{red}Z_k \color{black}\widetilde{Z}_kH(X_k-\widetilde{X}_k)Vv_k \end{aligned} XkZkX kA(Xk−1−X^k−1)Wwk−1Z kH(Xk−X k)Vvk 这样整个非线性系统就在 X ^ k − 1 \hat{X}_{k-1} X^k−1 和 X ~ k \widetilde{X}_k X k 附近进行线性化了然后这里面还要一个需要注意的地方我们知道 W W W 是服从正态分布的期望为 0 0 0协方差矩阵为 Q Q Q即 P ( W ) ∼ N ( 0 , Q ) P(W) \sim N(0,Q) P(W)∼N(0,Q)其实 W w Ww Ww 也是服从正态分布的它的期望依然是 0 0 0但它的协方差矩阵变成了 w Q w T wQw^T wQwT即 P ( W w ) ∼ N ( 0 , w Q w T ) P(Ww) \sim N(0,wQw^T) P(Ww)∼N(0,wQwT)
同理 P ( V v k ) ∼ N ( 0 , v R v T ) P(Vv_k) \sim N(0,vRv^T) P(Vvk)∼N(0,vRvT)
有了这样的结果后面的事情就理所当然了就可以用我们以前推过的手段来推导了我们来看下非线性的和线性的有什么区别。
首先预测部分我们可以直接用非线性化的系统进行预测
预测 X ^ k − f ( X ^ k − 1 , U k − 1 , 0 ) P k − A P k − 1 A T w Q w T \begin{aligned} \hat{X}_k^- \color{red}f(\hat{X}_{k-1},U_{k-1},0) \\ P_k^- AP_{k-1}A^T \color{red}wQw^T \end{aligned} X^k−Pk−f(X^k−1,Uk−1,0)APk−1ATwQwT 校正 K k P k − H T H P k − H T v R v T X ^ k X ^ k − K k ( Z k − h ( X ^ k − , 0 ) ) P k ( I − K k H ) P k − \begin{aligned} K_k \frac{P_k^-H^T}{HP_k^-H^T\color{red}vRv^T} \\ \hat{X}_k \hat{X}_k^- K_k(Z_k - \color{red}h(\hat{X}_k^-,0)\color{black}) \\ P_k (I - K_kH)P_k^- \end{aligned} KkX^kPkHPk−HTvRvTPk−HTX^k−Kk(Zk−h(X^k−,0))(I−KkH)Pk− 其中标红的部分代表和线性系统不一样的地方这就是一个新的扩展卡尔曼滤波器的应用方法
OK扩展卡尔曼滤波器的内容到这里就结束了
通过这几篇文章算是把卡尔曼滤波做了一个完整的介绍整个系统还是比较完整的希望大家学过以后会有一些帮助
3. 补充知识-线性化 视频链接【工程数学基础】2_线性化_泰勒级数_泰勒公式_Linearization 注博主也就把 DR_CAN 老师讲解的内容复述了一遍强烈建议大家观看原视频 我们会介绍一个通用的线性化的数学方法它不只是局限于控制理论方面的应用
我们首先来看什么是线性系统 Linear System线性系统符合叠加原理 superposition x 1 x_1 x1 x 2 x_2 x2 是解 x 3 k 1 x 1 k 2 x 2 x_3 k_1x_1 k_2x_2 x3k1x1k2x2 其中 k 1 k_1 k1 k 2 k_2 k2 为常数 x 3 x_3 x3 是解
符合以上三条的就是线性系统
举几个例子 x ¨ 2 x ˙ 2 x 0 x ¨ 2 x ˙ 2 x 2 0 x ¨ sin x ˙ 2 x 0 \begin{aligned} \ddot{x}2\dot{x}\sqrt{2}x 0 \\ \ddot{x}2\dot{x}\sqrt{2}x^2 0 \\ \ddot{x}\sin{\dot{x}}\sqrt{2}x 0 \end{aligned} x¨2x˙2 xx¨2x˙2 x2x¨sinx˙2 x000 其中第一个是线性的第二个和第三个不是大家可以用上面三条原则验证下可以发现不符合叠加原则 我们现在来看一下线性化的方法去线性化一个系统我们会用到一个重要的数学理论泰勒级数Taylor Series泰勒级数的展开式如下 f ( x ) f ( x 0 ) f ′ ( x 0 ) 1 ! ( x − x 0 ) f ′ ′ ( x 0 ) 2 ! ( x − x 0 ) 2 ⋯ f ( n ) ( x 0 ) n ! ( x − x 0 ) n f(x)f(x_0)\frac{f(x_0)}{1!}(x-x_0)\frac{f(x_0)}{2!}(x-x_0)^2\cdots\frac{f^{(n)}(x_0)}{n!}(x-x_0)^n f(x)f(x0)1!f′(x0)(x−x0)2!f′′(x0)(x−x0)2⋯n!f(n)(x0)(x−x0)n 如果 x − x 0 → 0 x-x_0 \to 0 x−x0→0则 ( x − x 0 ) 2 → 0 (x-x_0)^2 \to 0 (x−x0)2→0 ( x − x 0 ) n → 0 (x-x_0)^n \to 0 (x−x0)n→0这样的话在 x 0 x_0 x0 这点 f ( x ) f(x) f(x) 的展开就变成了如下形式 f ( x ) f ( x 0 ) f ′ ( x 0 ) ( x − x 0 ) \color{blue}f(x)f(x_0)f(x_0)(x-x_0) f(x)f(x0)f′(x0)(x−x0) 我们来看一下这个式子其中 f ( x 0 ) f(x_0) f(x0) 是一个常数我们令它等于 k 1 k_1 k1同理 f ′ ( x 0 ) f(x_0) f′(x0) 也是一个常数我们令它等于 k 2 k_2 k2则有 f ( x ) f ( x 0 ) f ′ ( x 0 ) ( x − x 0 ) k 1 k 2 ( x − x 0 ) k 1 k 2 x − k 2 x 0 k 2 x b \begin{aligned} f(x)f(x_0)f(x_0)(x-x_0) \\ k_1 k_2(x-x_0) \\ k_1 k_2x - k_2x_0 \\ k_2x b \end{aligned} f(x)f(x0)f′(x0)(x−x0)k1k2(x−x0)k1k2x−k2x0k2xb 这其实是一条直线的表达方式 k 2 k_2 k2 代表着斜率等于 tan θ \tan\theta tanθ b b b 代表着截距
所以说这样的一种方式就把原来的 f ( x ) f(x) f(x) 线性化成了 k 2 x b k_2xb k2xb 这样一种形式我们把这个过程就叫做线性化。
比如说我们来举一个例子 f ( x ) sin x f(x) \sin x f(x)sinx我们把上述公式带进去可以得到 f ( x ) sin x 0 cos x 0 ( x − x 0 ) f(x) \sin{x_0} \cos{x_0}(x-x_0) f(x)sinx0cosx0(x−x0) 当 x 0 0 x_0 0 x00 时 f ( x ) x f(x)x f(x)x也就是说在零点附近 sin x \sin{x} sinx 的线性化形式就等于 x x x我们来简单验证一下比如 sin π 6 1 2 \sin{\frac{\pi}{6}} \frac{1}{2} sin6π21 π 6 ≈ 3.14 6 0.52 \frac{\pi}{6}\approx\frac{3.14}{6}0.52 6π≈63.140.52二者的误差为 0.52 − 0.5 0.5 × 100 % 4 % \frac{0.52-0.5}{0.5}\times100\%4\% 0.50.52−0.5×100%4%
再比如说 sin π 4 2 2 ≈ 0.707 \sin{\frac{\pi}{4}}\frac{\sqrt2}{2}\approx0.707 sin4π22 ≈0.707 π 4 ≈ 3.14 4 0.785 \frac{\pi}{4}\approx\frac{3.14}{4}0.785 4π≈43.140.785这个时候二者的误差达到了 0.785 − 0.707 0.707 × 100 % 11 % \frac{0.785-0.707}{0.707}\times100\%11\% 0.7070.785−0.707×100%11%
这是因为我们在线性化的过程当中大家一定要记住我们忽略了后面的平方项 ( x − x 0 ) 2 (x-x_0)^2 (x−x0)2但这只是 x x x 与 x 0 x_0 x0 非常接近的情况下我们才可以这样做所以大家一定要记住这一点线性化是在某一点附近的线性化而不是全局的线性化这一点非常非常的重要
现在我们来看一个系统的例子我们要将这个系统在平衡点fixed point附近线性化 x ¨ x ˙ 1 x 1 \ddot{x}\dot{x}\frac{1}{x}1 x¨x˙x11 平衡点的求法就是令所有的导数项都为 0 0 0即 x ¨ x ˙ 0 \ddot{x}\dot{x}0 x¨x˙0有 1 x 1 \frac{1}{x}1 x11平衡点 x 0 1 x_01 x01
然后在 x 0 x_0 x0 附近我们对它进行线性化我们可以在 x 0 x_0 x0 附近令 x δ x 0 x d x_{\delta} x_0x_d xδx0xd其中 x d x_d xd 是一个比较小的值我们将其代入上式有 x ¨ δ x ˙ δ 1 x δ 1 \color{blue}\ddot{x}_{\delta}\dot{x}_{\delta}\frac{1}{x_{\delta}}1 x¨δx˙δxδ11 我们现在把非线性的项 1 x δ \color{blue}\frac{1}{x_{\delta}} xδ1 进行线性化用我们刚才的公式有 f ( x δ ) f ( x 0 ) f ′ ( x 0 ) ( x δ − x 0 ) 1 x δ 1 x 0 − 1 x 0 2 x d 1 − x d \begin{aligned} \color{sienna}f(x_{\delta})f(x_0)f(x_0)(x_{\delta}-x_0) \\ \color{sienna}\frac{1}{x_{\delta}} \frac{1}{x_0}-\frac{1}{x_0^2}x_d 1-x_d \end{aligned} f(xδ)xδ1f(x0)f′(x0)(xδ−x0)x01−x021xd1−xd 也就是 1 x δ \color{sienna}\frac{1}{x_{\delta}} xδ1 在 x 0 x_0 x0 附近的线性化就可以变成 1 − x d 1-x_d 1−xd我们再把 x ¨ δ x ¨ 0 x ¨ d x ¨ d \color{sienna}\ddot{x}_{\delta}\ddot{x}_0\ddot{x}_d\ddot{x}_d x¨δx¨0x¨dx¨d x ˙ δ x ˙ 0 x ˙ d x ˙ d \color{sienna}\dot{x}_{\delta}\dot{x}_0\dot{x}_d\dot{x}_d x˙δx˙0x˙dx˙d 代入有 x ¨ d x ˙ d 1 − x d 1 x ¨ d x ˙ d − x d 0 \begin{aligned} \color{sienna}\ddot{x}_d \dot{x}_d1-x_d 1 \\ \color{sienna}\ddot{x}_d \dot{x}_d-x_d 0 \end{aligned} x¨dx˙d1−xdx¨dx˙d−xd10 这就是线性化的结果 我们再来看下对于二维的情况在平衡点附近是如何进行线性化的对于下面的式子 x ˙ 1 f 1 ( x 1 , x 2 ) x ˙ 2 f 2 ( x 1 , x 2 ) \begin{aligned} \dot{x}_1 f_1(x_1,x_2) \\ \dot{x}_2 f_2(x_1,x_2) \end{aligned} x˙1x˙2f1(x1,x2)f2(x1,x2) 在平衡点附近有 [ x ˙ 1 d x ˙ 2 d ] [ ∂ f 1 ∂ x 1 ∂ f 1 ∂ x 2 ∂ f 2 ∂ x 1 ∂ f 2 ∂ x 2 ] [ x 1 d x 2 d ] ∣ x x 0 \begin{bmatrix} \dot{x}_{1d} \\ \dot{x}_{2d} \end{bmatrix} \begin{bmatrix} \frac{\partial{f_1}}{\partial{x_1}} \frac{\partial{f_1}}{\partial{x_2}}\\ \frac{\partial{f_2}}{\partial{x_1}} \frac{\partial{f_2}}{\partial{x_2}} \end{bmatrix} \begin{bmatrix} {x}_{1d} \\ {x}_{2d} \end{bmatrix}_{\vert{xx_0}} [x˙1dx˙2d][∂x1∂f1∂x1∂f2∂x2∂f1∂x2∂f2][x1dx2d]∣xx0 现在我们依然用上面的例子 x ¨ x ˙ 1 x 1 \ddot{x}\dot{x}\frac{1}{x}1 x¨x˙x11这个时候我们可以令 x 1 x x_1x x1x x 2 x ˙ x_2\dot{x} x2x˙我们就可以得到状态空间方程 x ˙ 1 x 2 x ˙ 2 x ¨ 1 − 1 x − x ˙ 1 − 1 x 1 − x 2 \begin{aligned} \dot{x}_1 x_2 \\ \dot{x}_2 \ddot{x} 1-\frac{1}{x}-\dot{x}1-\frac{1}{x_1}-x_2 \end{aligned} x˙1x˙2x2x¨1−x1−x˙1−x11−x2 我们现在先来寻找它的平衡点也就是令 x ˙ 1 0 \color{blue}\dot{x}_10 x˙10 x ˙ 2 0 \color{blue}\dot{x}_20 x˙20我们可以得到 x 20 0 \color{blue}x_{20}0 x200 x 10 1 \color{blue}{x}_{10}1 x101
我们现在把上式带进去求取矩阵中的四项其实就是一个雅可比矩阵我们现在的目的是求下面四项在平衡点 x x 0 x x_0 xx0 的值 [ ∂ f 1 ∂ x 1 ∂ f 1 ∂ x 2 ∂ f 2 ∂ x 1 ∂ f 2 ∂ x 2 ] \begin{bmatrix} \frac{\partial{f_1}}{\partial{x_1}} \frac{\partial{f_1}}{\partial{x_2}}\\ \frac{\partial{f_2}}{\partial{x_1}} \frac{\partial{f_2}}{\partial{x_2}} \end{bmatrix} [∂x1∂f1∂x1∂f2∂x2∂f1∂x2∂f2] 我们已知条件有 f 1 ( x 1 , x 2 ) x ˙ 1 x 2 f 1 ( x 1 , x 2 ) x ˙ 2 1 − 1 x 1 − x 2 \begin{aligned} f_1(x_1,x_2) \dot{x}_1 x_2 \\ f_1(x_1,x_2) \dot{x}_2 1 - \frac{1}{x_1}-x_2 \end{aligned} f1(x1,x2)f1(x1,x2)x˙1x2x˙21−x11−x2 还已知在平衡点处有 x ˙ 1 0 x ˙ 2 0 x 10 1 x 20 0 \begin{aligned} \dot{x}_1 0 \\ \dot{x}_2 0 \\ x_{10} 1 \\ x_{20} 0 \end{aligned} x˙1x˙2x10x200010 则根据已知条件和求导规则有 ∂ f 1 ∂ x 1 0 ∂ f 1 ∂ x 2 1 ∂ f 2 ∂ x 1 1 x 1 2 ∂ f 2 ∂ x 2 − 1 \begin{aligned} \frac{\partial{f_1}}{\partial{x_1}} 0\\ \frac{\partial{f_1}}{\partial{x_2}} 1\\ \frac{\partial{f_2}}{\partial{x_1}} \frac{1}{x_1^2}\\ \frac{\partial{f_2}}{\partial{x_2}} -1 \end{aligned} ∂x1∂f1∂x2∂f1∂x1∂f2∂x2∂f201x121−1 然后把平衡点处的值 x 20 0 \color{blue}x_{20}0 x200 x 10 1 \color{blue}{x}_{10}1 x101 可以有 [ x ˙ 1 d x ˙ 2 d ] [ 0 1 1 x 1 2 − 1 ] [ x 1 d x 2 d ] ∣ x x 0 [ 0 1 1 − 1 ] [ x 1 d x 2 d ] \begin{bmatrix} \dot{x}_{1d} \\ \dot{x}_{2d} \end{bmatrix} \begin{bmatrix} 0 1\\ \frac{1}{x_1^2} -1 \end{bmatrix} \begin{bmatrix} {x}_{1d} \\ {x}_{2d} \end{bmatrix}_{\vert{xx_0}} \begin{bmatrix} 0 1\\ 1 -1 \end{bmatrix} \begin{bmatrix} {x}_{1d} \\ {x}_{2d} \end{bmatrix} [x˙1dx˙2d][0x1211−1][x1dx2d]∣xx0[011−1][x1dx2d] 我们来看下 x ˙ 2 d x 1 d − x 2 d \color{red}\dot{x}_{2d}x_{1d}-x_{2d} x˙2dx1d−x2d又已知 x 1 x x_1x x1x x 2 x ˙ x_2\dot{x} x2x˙ x ˙ 1 x 2 \dot{x}_1x_2 x˙1x2 x ˙ 2 x ¨ \dot{x}_2\ddot{x} x˙2x¨
代入有 x ¨ d x d − x ˙ d \color{red}\ddot{x}_dx_d-\dot{x}_d x¨dxd−x˙d x ¨ d x ˙ d − x d 0 \color{red}\ddot{x}_d\dot{x}_d-x_d0 x¨dx˙d−xd0
我们可以发现和上面算出来的结果是一样的所以这就是一个二维系统线性化的方法 我们来做一下总结首先是线性化的公式 f ( x ) f ( x 0 ) f ′ ( x 0 ) ( x − x 0 ) \color{black}f(x)f(x_0)f(x_0)(x-x_0) f(x)f(x0)f′(x0)(x−x0) 但是要记住 x − x 0 → 0 x-x_0 \to 0 x−x0→0 我们才可以应用这个公式
第二个就是二维情况下在平衡点处线性化的一个方法 [ x ˙ 1 d x ˙ 2 d ] [ ∂ f 1 ∂ x 1 ∂ f 1 ∂ x 2 ∂ f 2 ∂ x 1 ∂ f 2 ∂ x 2 ] [ x 1 d x 2 d ] ∣ x x 0 \begin{bmatrix} \dot{x}_{1d} \\ \dot{x}_{2d} \end{bmatrix} \begin{bmatrix} \frac{\partial{f_1}}{\partial{x_1}} \frac{\partial{f_1}}{\partial{x_2}}\\ \frac{\partial{f_2}}{\partial{x_1}} \frac{\partial{f_2}}{\partial{x_2}} \end{bmatrix} \begin{bmatrix} {x}_{1d} \\ {x}_{2d} \end{bmatrix}_{\vert{xx_0}} [x˙1dx˙2d][∂x1∂f1∂x1∂f2∂x2∂f1∂x2∂f2][x1dx2d]∣xx0 博主观看完了视频后依旧存在着一些困惑因此不断向 chatGPT 咨询以下均来自博主与 chatGPT 之间的对话 问题1什么叫做线性化 回答线性化是一个过程通过这个过程可以将非线性系统近似为线性系统。这通常是为了简化分析和设计因为线性系统的数学处理相对简单。 问题2怎么才叫做把一个非线性化的系统线性化了 回答将一个非线性系统线性化意味着用一个线性系统来近似原始的非线性系统。这通常在非线性系统的某个工作点附近进行。泰勒级数是一种常用的方法通过该方法可以将非线性函数展开为多项式形式然后保留其中的一些主要项来得到线性近似。 问题3什么叫做平衡点 回答平衡点也称为稳定点或工作点是系统中的一个点在该点上系统的所有导数或差分为零系统不会自发地改变其状态。对于动态系统平衡点是系统在没有外部干扰地情况下保持其状态不变的点。 问题4为什么要在平衡点处进行线性化 回答在平衡点处进行线性化是因为平衡点是系统自然存在的稳定状态没有外部干扰时系统会停留在这个状态。在此点附近的小扰动可以用线性系统来描述使得线性点近似更为准确。此外对于许多应用如控制系统设计我们通常对系统在工作点或平衡点附近的行为感兴趣因此在这些点进行线性化是有意义的。 结语 本篇博客主要通过一个实际的二维例子让我们更加直观的理解了卡尔曼滤波器在调参过程中主要对过程噪声协方差矩阵 Q Q Q 和测量噪声协方差矩阵 R R R 进行调整以得到更好的估计值。接着我们简单介绍了在非线性系统中如何去应用卡尔曼滤波器介绍了一个非常基础的方法将非线性系统线性化我们使用泰勒展开在系统平衡点处进行线性化这样的滤波器也叫做扩展卡尔曼滤波器。 卡尔曼滤波器的相关知识分享到这里就结束了后续如果还有其它的内容再补充吧 参考
常用的MarkDown颜色 笔记【卡尔曼滤波器】_Kalman_Filter_全网最详细数学推导【博主强烈推荐】【工程数学基础】2_线性化_泰勒级数_泰勒公式_Linearization
