电商网站建设开题报告,玻璃制品东莞网站建设,做网站内容,网站设计文档模板文章目录1. 线性系统的 Kalman Filter 回顾2. Extended Kalman Filter 之 DR_CAN讲解笔记2.1. 非线性系统2.2. 非线性系统线性化2.2.1. 状态方程f(xk)f(x_k)f(xk)在上一次的最优估计状态x^k−1\hat{x}_{k-1}x^k−1处线性化2.2.2. 观测方程h(xk)h(x_k)h(xk)在这一次的预测… 文章目录1. 线性系统的 Kalman Filter 回顾2. Extended Kalman Filter 之 DR_CAN讲解笔记2.1. 非线性系统2.2. 非线性系统线性化2.2.1. 状态方程f(xk)f(x_k)f(xk)在上一次的最优估计状态x^k−1\hat{x}_{k-1}x^k−1处线性化2.2.2. 观测方程h(xk)h(x_k)h(xk)在这一次的预测状态x~k\tilde{x}_kx~k处线性化2.3. Extended Kalman Filter3. Extended Kalman Filter 之 我的理解与详细解释1. 线性系统的 Kalman Filter 回顾 系统状态空间方程 xkAxk−1BUk−1wk−1P(ω)∼N(0,Q)zkHxkHkP(v)∼N(0,R)\begin{array}{ll} x_kA x_{k-1}B U_{k-1}w_{k-1} P(\omega) \sim N(0, Q) \\ z_kH x_kH_k P(v) \sim N(0, R) \end{array} xkAxk−1BUk−1wk−1zkHxkHkP(ω)∼N(0,Q)P(v)∼N(0,R) 预测 x^k−Ax^k−1Buk−1Pk−APk−1A⊤Q\begin{aligned} \hat{x}_k^{-}A \hat{x}_{k-1}B u_{k-1} \\ P_k^{-}A P_{k-1} A^{\top}Q \end{aligned} x^k−Ax^k−1Buk−1Pk−APk−1A⊤Q 矫正更新 KkPk−H⊤HPk−H⊤RY^kX^k−Kk(Zk−HX^k−)Pk(I−KkH)Pk−\begin{aligned} K_k\frac{P_k^{-} H^{\top}}{H P_k^{-} H^{\top}R} \\ \hat{Y}_k\hat{X}_k^{-}K_k\left(Z_k-H \hat{X}_k^{-}\right) \\ P_k\left(I-K_k H\right) P_k^{-} \end{aligned} KkHPk−H⊤RPk−H⊤Y^kX^k−Kk(Zk−HX^k−)Pk(I−KkH)Pk−
2. Extended Kalman Filter 之 DR_CAN讲解笔记
2.1. 非线性系统
对于非线性系统来说系统数学模型和观测模型无法用状态空间方程来表达因为状态空间方程是线性方程而非线性系统的这两个模型都是非线性的。可以写成如下形式 xkf(xk−1,uk−1,ωk−1)P(ω)∼N(0,Q)zkh(xk,vk)P(v)∼N(0,R)\begin{aligned} x_kf\left(x_{k-1}, u_{k-1}, \omega_{k-1}\right) P(\omega) \sim N(0, Q)\\ z_kh\left(x_k, v_k\right) P(v) \sim N(0, R) \end{aligned} xkf(xk−1,uk−1,ωk−1)zkh(xk,vk)P(ω)∼N(0,Q)P(v)∼N(0,R)
注意上述的高斯噪声是在非线性函数里面的这样即使噪声原本是高斯的但是经过非线性系统之后它的分布也不再是高斯分布了如下图所示。 可以发现Kalman Filter的两个前提条件1线性系统2高斯噪声已经全都不满足了。
所以对于非线性系统应用 Kalman Filter 的最好方法就是对系统在工作点/线性化点 进行线性化因为只有线性化之后我们才能写成线性的形式才能得到系数矩阵AAA和HHH进而计算Kalman Gain最后进行数据融合。
2.2. 非线性系统线性化
线性化需要在一个线性化点进行对于非线性系统的Kalman Filter来说最准确的地方就是在真实状态点进行线性化。
但是真实状态点到底是多少我们永远都不知道因为如果知道了就不需要做 Kalman Filter了。所以我们只能在我们知道的工作点处进行线性化我们知道的工作点有两个
上一次的最优估计状态x^k−1\hat{x}_{k-1}x^k−1这一次的先验估计状态x^k−\hat{x}_k^-x^k−
2.2.1. 状态方程f(xk)f(x_k)f(xk)在上一次的最优估计状态x^k−1\hat{x}_{k-1}x^k−1处线性化 2.2.2. 观测方程h(xk)h(x_k)h(xk)在这一次的预测状态x~k\tilde{x}_kx~k处线性化 2.3. Extended Kalman Filter 3. Extended Kalman Filter 之 我的理解与详细解释
既然是卡尔曼滤波自然就是两个方程状态方程和观测方程。由于卡尔曼滤波是离散的所以下面我们先给出IMU的离散状态空间方程和状态观测方程如下
xkf(xk−1,wk−1)zkh(xk,vk)\boldsymbol x_{k} \boldsymbol f(\boldsymbol x_{k-1} , \boldsymbol w_{k-1}) \\ \boldsymbol z_{k} \boldsymbol h(\boldsymbol x_{k}, \boldsymbol v_{k} ) \\ xkf(xk−1,wk−1)zkh(xk,vk)
由于这两个方程都是非线性方程所以为了使用使用卡尔曼滤波必须对他们进行线性化。并且由于线性化必须知道线性化工作点而我们实际知道的都是名义状态值这里的名义值包括 上一次的状态后验值 xˇk−1\boldsymbol {\check{x}}_{k-1}xˇk−1 和 这一次的状态先验值 x^k\boldsymbol {\hat{x}}_{k}x^k。所以对上面两个公式进行线性化也都是在 名义状态 的地方进行线性化得到如下的公式
x^kf(x^k−1,0)Fk−1(xk−1−xˇk−1)Wk−1wk−1zkh(x^k,0)Hk(xk−x^k)Vkvk\boldsymbol {\hat{x}}_{k} \boldsymbol f(\boldsymbol {\hat{x}}_{k-1} , \boldsymbol 0) \boldsymbol F_{k-1} (\boldsymbol x_{k-1} - \boldsymbol {\check{x}}_{k-1}) \boldsymbol W_{k-1} \boldsymbol w_{k-1} \\ \boldsymbol z_{k} \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0) \boldsymbol H_{k} (\boldsymbol x_{k} - \boldsymbol {\hat{x}}_{k}) \boldsymbol V_{k} \boldsymbol v_{k} x^kf(x^k−1,0)Fk−1(xk−1−xˇk−1)Wk−1wk−1zkh(x^k,0)Hk(xk−x^k)Vkvk
注意
1 时刻记住我们线性化的关键目的是什么是为了得到系数矩阵从而变成线性系统可以使用典型的线性系统卡尔曼滤波。
2 上面的方程线性化出来的两个固定函数值 f(x^k−1,0)\boldsymbol f(\boldsymbol {\hat{x}}_{k-1} , \boldsymbol 0)f(x^k−1,0) 和 h(x^k,0)\boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0)h(x^k,0) 对我们使用卡尔曼滤波没有影响因为我们计算协方差矩阵的时候是使用系数矩阵来计算的。
3假设传感器观测到的值是zm\boldsymbol z_mzm这里省略掉了时间下标kkk然后用下标mmm表示是 观测的测量值。然后用z\boldsymbol zz代表预测观测值也就是我们把IMU预测得到的状态先验状态x^k\boldsymbol {\hat{x}}_{k}x^k 带入到观测方程中得到的 计算出来的观测值。因为看上面的典型的卡尔曼滤波公式就知道我们是把IMU 先验状态 带入观测方程中得到一个 计算出来的预测观测值然后和真正的观测传感器的测量值作差再乘以卡尔曼增益进行校正。所以这里计算的 预测观测值 表达公式如下因为我们在计算噪声不知道是多少所以简化为0 zh(x^k,0)Hk(x^k−x^k)Vk0h(x^k,0)\boldsymbol z \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0) \boldsymbol H_{k} (\boldsymbol {\hat{x}}_{k} - \boldsymbol {\hat{x}}_{k}) \boldsymbol V_{k} \boldsymbol 0 \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0) zh(x^k,0)Hk(x^k−x^k)Vk0h(x^k,0)
4重要上面的公式中xk−1\boldsymbol {x}_{k-1}xk−1 和 xk\boldsymbol {x}_{k}xk 是 自变量x^k\boldsymbol {\hat x}_kx^k 和 zk\boldsymbol z_{k}zk 是 函数值所以我们带入不同的自变量值会得到不同的函数值。比如我们实际计算的时候在预测方程中带入的是 上一次的后验状态值xˇk−1\boldsymbol {\check{x}}_{k-1}xˇk−1那么得到的就是 本次的先验状态值 x^kf(x^k−1,0)\boldsymbol{\hat{x}}_{k} \boldsymbol f(\boldsymbol {\hat{x}}_{k-1}, \boldsymbol 0)x^kf(x^k−1,0)我们在观测方程中带入 本次的先验状态值得到的就是 本次的先验预测观测值zkh(x^k,0)\boldsymbol z_{k} \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0)zkh(x^k,0)。
5注意在我们计算卡尔曼滤波的时候我们只能按照4中所说的那样带入 上一次的先验状态到状态方程然后带入 这一次的先验状态到观测方程。因为在典型的线性卡尔曼滤波中就是这么做的我们就是要融合状态方程和观测方程。
最后给出EKF的卡尔曼滤波方程为
预测公式 带入上一时刻的后验状态到自变量中即xk−1←xˇk−1:x^kf(x^k−1,0)Fk−1(xˇk−1−xˇk−1)f(x^k−1,0)P^kFk−1Pˇk−1Fk−1TQ带入上一时刻的后验状态到自变量中即\boldsymbol x_{k-1} \leftarrow \boldsymbol {\check{x}}_{k-1} : \\\boldsymbol {\hat{x}}_{k} \boldsymbol f(\boldsymbol {\hat{x}}_{k-1} , \boldsymbol 0) \boldsymbol F_{k-1} (\boldsymbol {\check{x}}_{k-1} - \boldsymbol {\check{x}}_{k-1}) \boldsymbol f(\boldsymbol {\hat{x}}_{k-1} , \boldsymbol 0) \\ \ \boldsymbol {\hat P}_{k} \boldsymbol F_{k-1} \boldsymbol {\check P}_{k-1} \boldsymbol F_{k-1} ^{T} \boldsymbol Q 带入上一时刻的后验状态到自变量中即xk−1←xˇk−1:x^kf(x^k−1,0)Fk−1(xˇk−1−xˇk−1)f(x^k−1,0) P^kFk−1Pˇk−1Fk−1TQ
校正公式 带入这一时刻的先验状态到自变量中即xk←x^k:zkh(x^k,0)Hk(x^k−x^k)h(x^k,0)KkP^kHTHP^kHTRxˇkx^kKk(zm−h(x^k,0))Pˇk(I−KkH)P^k带入这一时刻的先验状态到自变量中即\boldsymbol x_{k} \leftarrow \boldsymbol {\hat{x}}_{k} : \\ \boldsymbol z_{k} \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0) \boldsymbol H_{k} (\boldsymbol {\hat{x}}_{k} - \boldsymbol {\hat{x}}_{k}) \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0) \\ \begin{gathered} \boldsymbol K_{k}\frac{ \boldsymbol {\hat P}_{k} \boldsymbol H^{T}}{ \boldsymbol H \boldsymbol {\hat P}_{k} \boldsymbol H^{T} \boldsymbol R} \\ \boldsymbol {\check {x}}_{k} \boldsymbol {\hat{x}}_{k} \boldsymbol {K}_{k}\left( \boldsymbol {z}_m - \boldsymbol h(\boldsymbol {\hat{x}}_{k}, \boldsymbol 0) \right) \\ \boldsymbol {\check P}_{k}\left( \boldsymbol I- \boldsymbol K_{k} \boldsymbol H\right) \boldsymbol {\hat P}_{k} \end{gathered} 带入这一时刻的先验状态到自变量中即xk←x^k:zkh(x^k,0)Hk(x^k−x^k)h(x^k,0)KkHP^kHTRP^kHTxˇkx^kKk(zm−h(x^k,0))Pˇk(I−KkH)P^k
