1.非线性体系的线性化
- 尺度卡尔曼滤波 实用于线性化体系，扩展卡尔曼滤波 则扩展到了非线性体系，核心原理就是将非线性体系线性化，告急用的的知识点是 泰勒睁开（我别的一篇文章的链接），如下是泰勒睁开的公式：
P n ( x ) = f ( a ) + f ′ ( a ) ( x − a ) + f ′ ′ ( a ) 2 ! ( x − a ) 2 + ⋯ + f ( n ) ( a ) n ! ( x − a ) n P_n(x)=f(a)+f'(a)(x-a)+\frac{f''(a)}{2!}(x-a)^2+\cdots+\frac{f^{(n)}(a)}{n!}(x-a)^n Pn(x)=f(a)+f′(a)(x−a)+2!f′′(a)(x−a)2+⋯+n!f(n)(a)(x−a)n
- 如下图所示 泰勒睁开 公式所示，只看红框部分（不思量高阶），其本身就是一个线性化的公式。
- 以下是 y = sin ( x ) y\:=\:\sin(x) y=sin(x) 和他的一阶 泰勒睁开，显然，他是线性的。
- 团结 泰勒睁开 中的形貌，一阶泰勒公式只有 在他附近的点上是相对准确的，应注意。
2. 扩展卡尔曼滤波的状态空间方程
2.1 尺度卡尔曼滤波的状态空间方程
- 关于尺度卡尔曼滤波可以参考问哦的别的一篇文章 尺度卡尔曼滤波 。
x k = A x k − 1 + B u k − 1 + w k − 1 / / 状态转移方程 − − − − z k = H x k + v k / / 观测方程 x_k = A x_{k-1} + Bu_{k-1} + w_{k-1} // 状态转移方程\\ ----\\ z_k = Hx_k + v_k //观测方程 xk=Axk−1+Buk−1+wk−1//状态转移方程−−−−zk=Hxk+vk//观测方程
2.1 扩展卡尔曼滤波的状态空间方程
x k = f ( x k − 1 , u k − 1 , w k − 1 ) / / 状态方程 − − − − z k = h ( x k , v k ) / / 观测方程 x_k=f(x_{k-1},u_{k-1}, w_{k-1}) // 状态方程\\ ----\\ z_k=h(x_k, v_k) //观测方程 xk=f(xk−1,uk−1,wk−1)//状态方程−−−−zk=h(xk,vk)//观测方程
- f f f 和 h h h 为非线性函数；
- w k w_k wk: 过程噪声, 假设符合正态分布, w k ∼ N ( 0 , Q ) w_k\sim\mathcal{N}(0,Q) wk∼N(0,Q)， 数学渴望为0，毛病的协方差矩阵为 Q Q Q
- v k v_k vk: 丈量噪声, 假设概率分布符合正态分布， v k ∼ N ( 0 , R ) v_k\sim\mathcal{N}(0,R) vk∼N(0,R),数学渴望为0，毛病的协方差矩阵为 R R R
3. 状态方程和观测方程的线性化
- 使用 泰勒睁开 举行线性化，即在一个点处睁开，理想情况是在真实点处睁开，但我们并不知道真实点在哪（假如知道就不消卡尔曼滤波了），退而求其次，此时的最优估计点就是 当前时候的 后验估计，通过在 k − 1 k-1 k−1 时候的后验估计点睁开， 得到 k k k 时候的状态。
- 状态方程 的线性化
x k = f ( x ^ k − 1 , u k − 1 , 0 ) + A ( x k − x ^ k − 1 ) + W k w k − 1 x_k=f( \hat{\mathbf{x}}_{k-1},u_{k-1}, 0) + A(x_k - \hat{\mathbf{x}}_{k-1}) + W_k w_{k-1} xk=f(x^k−1,uk−1,0)+A(xk−x^k−1)+Wkwk−1
- 在原来非线性化的方程（ x k = f ( x k − 1 , u k − 1 , w k − 1 ) x_k=f(x_{k-1},u_{k-1}, w_{k-1}) xk=f(xk−1,uk−1,wk−1) ）中, 是由上一时候的真实值 x k − 1 x_{k-1} xk−1 得到下一时候的真实值 x k x_k xk, 但在举行线性化是是没有办法得到真实值的，以是就使用 x k − 1 x_{k-1} xk−1 时候的 后验估计 处睁开。
- A A A：状态转移函数 f f f 对状态 x ^ k − 1 \hat{\mathbf{x}}_{k-1} x^k−1 的雅可比矩阵, 也就是 f f f 在 x ^ k − 1 \hat{\mathbf{x}}_{k-1} x^k−1 这个点求偏导 A = ∂ f ( x ^ k − 1 , u k − 1 , 0 ) ∂ x A=\frac{\partial f(\hat{\mathbf{x}}_{k-1},u_{k-1}, 0)}{\partial x} A=∂x∂f(x^k−1,uk−1,0)，
- W k W_k Wk： f f f 对 w k − 1 w_{k-1} wk−1 的偏导， W k = ∂ f ( x ^ k − 1 , u k − 1 , 0 ) ∂ w W_k=\frac{\partial f(\hat{\mathbf{x}}_{k-1},u_{k-1}, 0)}{\partial w} Wk=∂w∂f(x^k−1,uk−1,0)
- W k w k − 1 W_k w_{k-1} Wkwk−1 符合正态分布数学渴望为 0 0 0，协方差矩阵为 W Q W T WQW^T WQWT
W k w k − 1 ∼ N ( 0 , W Q W T ) W_k w_{k-1}\sim\mathcal{N}(0,WQW^T) Wkwk−1∼N(0,WQWT)
- 观测方程的线性化
z k = h ( f ( x ^ k − 1 , u k − 1 , 0 ) , 0 ) + H ( x k − f ( x ^ k − 1 , u k − 1 , 0 ) ) + V v k z_k = h(f( \hat{\mathbf{x}}_{k-1},u_{k-1}, 0),0) + H(x_k - f( \hat{\mathbf{x}}_{k-1},u_{k-1}, 0)) + Vv_k zk=h(f(x^k−1,uk−1,0),0)+H(xk−f(x^k−1,uk−1,0))+Vvk
- 同样的在观测方程中也是找不到真实点的，同样是在 k − 1 k-1 k−1 时候的 后验估计 处睁开，得到近似的 真实点 的位置。
- H = ∂ h ( f ( x ^ k − 1 , u k − 1 , 0 ) , 0 ) ∂ x H =\frac{\partial h(f( \hat{\mathbf{x}}_{k-1},u_{k-1}, 0),0) }{\partial x} H=∂x∂h(f(x^k−1,uk−1,0),0)
- V k = ∂ h ( f ( x ^ k − 1 , u k − 1 , 0 ) , 0 ) ∂ v V_k =\frac{\partial h(f( \hat{\mathbf{x}}_{k-1},u_{k-1}, 0),0) }{\partial v} Vk=∂v∂h(f(x^k−1,uk−1,0),0)
- V v k Vv_k Vvk 符合正态分布数学渴望为 0 0 0，协方差矩阵为 V R V T VRV^T VRVT
V v k ∼ N ( 0 , V R V T ) Vv_k\sim\mathcal{N}(0,VRV^T) Vvk∼N(0,VRVT)
- 为了方便明确，上面线性化后的公式并没有化简，通常们会写成如下情势：
- 令 x ~ k = f ( x ^ k − 1 , u k − 1 , 0 ) \tilde{x}_k = f( \hat{\mathbf{x}}_{k-1},u_{k-1}, 0) x~k=f(x^k−1,uk−1,0) 状态方程和观测方程就成为如下情势：
x k = x ~ k + A ( x k − x ^ k − 1 ) + W k w k − 1 x_k=\tilde{x}_k + A(x_k - \hat{\mathbf{x}}_{k-1}) + W_k w_{k-1} xk=x~k+A(xk−x^k−1)+Wkwk−1
z k = h ( x ~ k , 0 ) + H ( x k − x ~ k ) + V v k z_k = h(\tilde{x}_k,0) + H(x_k - \tilde{x}_k) + Vv_k zk=h(x~k,0)+H(xk−x~k)+Vvk
- 令 z ~ k = h ( x ~ k , 0 ) \tilde{z}_k =h(\tilde{x}_k,0) z~k=h(x~k,0)状态方程和观测方程就成为如下情势： x k = x ~ k + A ( x k − x ^ k − 1 ) + W k w k − 1 x_k=\tilde{x}_k + A(x_k - \hat{\mathbf{x}}_{k-1}) + W_k w_{k-1} xk=x~k+A(xk−x^k−1)+Wkwk−1
z k = z ~ k + H ( x k − x ~ k ) + V v k z_k = \tilde{z}_k + H(x_k - \tilde{x}_k) + Vv_k zk=z~k+H(xk−x~k)+Vvk
4.扩展卡尔曼滤波的递推流程
- 扩展卡尔曼滤波的递推流程在尺度卡尔曼滤波的根本上举行修改我把尺度卡尔曼滤波的递推流程也写在此处，方便对比
4.1 尺度卡尔曼滤波的递推流程
- 先验估计
x ^ k − = F x ^ k − 1 + B u k − 1 / / 状态的先验估计 \hat{\mathbf{x}}_k^-=\mathbf{F} \hat{\mathbf{x}}_{k-1}+\mathbf{B} \mathbf{u}_{k-1} //状态的先验估计 x^k−=Fx^k−1+Buk−1//状态的先验估计
P k − = F k P k − 1 F k ⊤ + Q k / / 毛病协方差矩阵的先验估计 \mathbf{P}_k^-=\mathbf{F}_k\mathbf{P}_{k-1}\mathbf{F}_k^\top+\mathbf{Q}_k // 毛病协方差矩阵的先验估计 Pk−=FkPk−1Fk⊤+Qk//毛病协方差矩阵的先验估计
- 后验估计
K k = P k − H T H P k − H T + R k / / 卡尔曼增益 K_k=\frac{P_{k}^{-}H^{T}}{HP_{k}^{-}H^{T}+R_k} //卡尔曼增益 Kk=HPk−HT+RkPk−HT//卡尔曼增益
x ^ k = x ^ k − + k k ( z k − H x ^ k − ) / / 后验估计 ( 当前时候的最优估计 ) \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + k_k (z_k - H\hat{\mathbf{x}}_k^-) //后验估计(当前时候的最优估计) x^k=x^k−+kk(zk−Hx^k−)//后验估计(当前时候的最优估计)
P k = ( I − K k H ) P k − / / 更新毛病的协方差矩阵 ; I : 单位矩阵 P_{k}=(I-K_{k}H)P_{k}^{-} // 更新毛病的协方差矩阵 ; I:单位矩阵 Pk=(I−KkH)Pk−//更新毛病的协方差矩阵;I:单位矩阵
4.2 扩展卡尔曼滤波的递推流程
- 先验估计
x ^ k − = f ( x ^ k − 1 , u k − 1 , 0 ) / / 直接使用非线性的公式举行先验估计 \hat{\mathbf{x}}_k^- = f( \hat{\mathbf{x}}_{k-1},u_{k-1}, 0) //直接使用非线性的公式举行先验估计 x^k−=f(x^k−1,uk−1,0)//直接使用非线性的公式举行先验估计
P k − = F k P k − 1 F k ⊤ + W Q W T / / 毛病协方差矩阵的先验估计 \mathbf{P}_k^-=\mathbf{F}_k\mathbf{P}_{k-1}\mathbf{F}_k^\top+WQW^T // 毛病协方差矩阵的先验估计 Pk−=FkPk−1Fk⊤+WQWT//毛病协方差矩阵的先验估计
- 后验估计
K k = P k − H T H P k − H T + V R V T / / 卡尔曼增益 K_k=\frac{P_{k}^{-}H^{T}}{HP_{k}^{-}H^{T}+VRV^T} //卡尔曼增益 Kk=HPk−HT+VRVTPk−HT//卡尔曼增益
x ^ k = x ^ k − + k k ( z k − h ( x ^ k − , 0 ) ) / / 后验估计 ( 当前时候的最优估计 ) \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + k_k (z_k -h(\hat{\mathbf{x}}_k^-,0) ) //后验估计(当前时候的最优估计) x^k=x^k−+kk(zk−h(x^k−,0))//后验估计(当前时候的最优估计)
P k = ( I − K k H ) P k − / / 更新毛病的协方差矩阵 ; I : 单位矩阵 P_{k}=(I-K_{k}H)P_{k}^{-} // 更新毛病的协方差矩阵 ; I:单位矩阵 Pk=(I−KkH)Pk−//更新毛病的协方差矩阵;I:单位矩阵
免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。 |
发表于 2025-10-11 07:43:03