扩展卡尔曼滤波器设计

对于线性系统，状态方程和观测方程如下：

Xk=AXk−1+Buk−1+Wk−1Zk=Hk−1+Vk\begin{aligned}&X_{k}=A X_{k-1}+B u_{k-1}+W_{k-1} \\&Z_{k}=H_{k-1}+V_{k}\end{aligned} Xk=AXk−1+Buk−1+Wk−1Zk=Hk−1+Vk

其中

P(w)∼N(0,Q)P(v)∼N(0,R)\begin{aligned}&P(w) \sim N(0, Q) \\&P(v) \sim N(0, R)\end{aligned}P(w)∼N(0,Q)P(v)∼N(0,R)

预测方程

x^k−=Ax^k+1+Buk−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+1+Buk−1Pk−=APk−1A⊤+Q

校正方程

kk=Pk−H⊤HPk−H⊤+Rx^k=x^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{x}_{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} kkx^kPk=HPk−H⊤+RPk−H⊤=x^k−kk(zk−Hx^k−)=(I−kkH)Pk

对于非线性系统，有以下表达形式：

xk=f(xk−1,ωk−1,wk−1)zk=h(xk,vk)\begin{aligned}&x_{k}=f\left(x_{k-1}, \omega_{k-1}, w_{k-1}\right) \\&z_{k}=h\left(x_{k}, v_{k}\right)\end{aligned} xk=f(xk−1,ωk−1,wk−1)zk=h(xk,vk)

f, h 为非线性函数

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

如果对于以上系统使用卡尔曼滤波，则需进行线性化（泰勒线性近似）

采用方法：泰勒展开

f(x)=f(λ0)+∂f∂x(x−x0)f(x)=f\left(\lambda_{0}\right)+\frac{\partial f}{\partial x}\left(x-x_{0}\right) f(x)=f(λ0)+∂x∂f(x−x0)

以上展开式对于二维函数采用求导方式，对于高维函数采用求 Jacobian 矩阵方式

（这部分参考 DR_CAN 的工程数学基础——线性化与泰勒级数）

已知问题：系统有误差，无法在真实点线性化

因此，过程方程函数f(xk)f(x_{k})f(xk)在x^k−1\hat{x}_{k-1}x^k−1（k-1处的后验估计）处线性化

xk=f(x^k−1,uk−1,ωk−1)+A(xk−x^k−1)+ωkωk−1x_{k}=f\left(\hat{x}_{k-1}, u_{k-1}, \omega_{k-1}\right)+A\left(x_{k}-\hat{x}_{k-1}\right)+\omega_{k} \omega_{k-1} xk=f(x^k−1,uk−1,ωk−1)+A(xk−x^k−1)+ωkωk−1

ωk−1\omega_{k-1}ωk−1为误差项，此处假设为0

令f(x^k−1,uk−1,0)=x~kf(\hat{x}_{k-1},u_{k-1},0)=\tilde{x}_{k}f(x^k−1,uk−1,0)=x~k

A=∂f∂x∣x^k−1,uk−1A=\frac{\partial f}{\partial x} \mid \hat{x}_{k-1}, u_{k-1}A=∂x∂f∣x^k−1,uk−1（求 k-1 点处的 Jacobian 矩阵）

A矩阵随k的变化而不断变化

对于噪声方程：

Wk=∂f∂w∣x^k−1uk−1W_{k}=\frac{\partial f}{\partial w} \mid \hat{x}_{k-1} u_{k-1}Wk=∂w∂f∣x^k−1uk−1

观测函数zkz_{k}zk在x~k\tilde{x}_{k}x~k处线性化

zk=h(x~k,vk)+H(xk−x~k)+vvkz_{k}=h\left(\tilde{x}_{k}, v_{k}\right)+H\left(x_{k}-\tilde{x}_{k}\right)+v_{v_{k}} zk=h(x~k,vk)+H(xk−x~k)+vvk

vkv_{k}vk为误差项，此处假设为0

h(x~k2,0)=z~k2h\left(\tilde{x}_{k}^{2}, 0\right)=\tilde{z}_{k}^{2}h(x~k2,0)=z~k2

H=∂h∂x∣x~kV=∂h∂v∣x~k\begin{aligned}&H=\frac{\partial h}{\partial x} \mid \tilde{x}_{k} \\&V=\frac{\partial h}{\partial v} \mid \tilde{x}_{k}\end{aligned}H=∂x∂h∣x~kV=∂v∂h∣x~k

将xkx_{k}xk和zkz_{k}zk非线性系统函数在 k-1 处线性化，得出

Xk=x~k+A(xk−x^k−1)+Wωk−1Zk=z~k+H(xk−x~k)+Vvk\begin{aligned}&X_{k}=\tilde{x}_{k}+A\left(x_{k}-\hat{x}_{k-1}\right)+W \omega_{k-1} \\&Z_{k}=\tilde{z}_{k}+H\left(x_{k}-\tilde{x}_{k}\right)+V v_{k}\end{aligned} Xk=x~k+A(xk−x^k−1)+Wωk−1Zk=z~k+H(xk−x~k)+Vvk

对于噪声函数：

P(ω)∼N(0,Q)P(ωω)∼N(0,ωQω⊤)P(\omega) \sim N(0, Q) \\P\left(\omega_{\omega}\right) \sim N\left(0, \omega Q \omega^{\top}\right)P(ω)∼N(0,Q)P(ωω)∼N(0,ωQω⊤)

综上所述，对于非线性系统来说

非线性预测方程为

x^k−=f(xk−12,uk−1,0)Pk−=APk−1A⊤+WQω⊤\begin{aligned}&\hat{x}_{k}^{-}=f\left(x_{k-1}^{2}, u_{k-1}, 0\right) \\&P_{k}^{-}=A P_{k-1} A^{\top}+W Q \omega^{\top}\end{aligned} x^k−=f(xk−12,uk−1,0)Pk−=APk−1A⊤+WQω⊤

非线性校正方程为
kk=Pk−H⊤HPk−H⊤+VRV⊤x^k=x^k−+kk(zk−h(x^k,0))Pk=(I−kkH)Pk−\begin{aligned}k_{k} &=\frac{P_{k}^{-} H^{\top}}{H P_{k}-H^{\top}+V R V^{\top}} \\\hat{x}_{k} &=\hat{x}_{k}^{-}+k_{k}\left(z_{k}-h\left(\hat{x}_{k}, 0\right)\right) \\P_{k} &=\left(I-k_{k} H\right) P_{k}^{-}\end{aligned} kkx^kPk=HPk−H⊤+VRV⊤Pk−H⊤=x^k−+kk(zk−h(x^k,0))=(I−kkH)Pk−

扩展卡尔曼滤波器设计相关推荐

【Matlab】扩展卡尔曼滤波器原理及仿真（初学者入门专用）
文章目录 0.引言及友情链接 1.场景预设 2.扩展卡尔曼滤波器 3.仿真及效果 0.引言及友情链接 \qquad卡尔曼滤波器(Kalman Filter, KF)是传感器融合(Sensor Fusi ...
卡尔曼滤波器、扩展卡尔曼滤波器、无向卡尔曼滤波器的详细推导
这段时间做轴承故障诊断和预测的时候,需要一个针对已经获取了特征向量的工具来对轴承故障状态进行估计和预测.卡尔曼滤波器可以实现对过去.当前和未来目标位置的估计,所以想通过卡尔曼滤波器的设计思路找到一些灵 ...
由浅入深的扩展卡尔曼滤波器教程
本篇译文翻译自 The Extended Kalman Filter : An Interactive Turorial for Non-Experts. 原文本文惯例及说明 : 译文中的Demo请 ...
卡尔曼转矩观测_基于扩展卡尔曼滤波器的永磁同步电机转速和磁链观测器
基于扩展卡尔曼滤波器的永磁同步电机转速和磁链观测器第六图书馆为了取消永磁同步电机控制中的机械传感器,获得直接转矩控制中需要的电机磁链信息,设计了一种基于扩展卡尔曼滤波器的永磁同步电机转速和磁链估 ...
【滤波】扩展卡尔曼滤波器（EKF）
%matplotlib inline #format the book import book_format book_format.set_style() 我们发展了线性卡尔曼滤波器的理论.然后在上 ...
【信号处理】基于扩展卡尔曼滤波器和无迹卡尔曼滤波器的窄带信号时变频率估计（Matlab代码实现）
目录 1 概述 2 数学模型 3 运行结果 4 结论 5 参考文献 6 Matlab代码实现 1 概述本文讲解和比较了基于卡尔曼滤波器的频率跟踪方法的能力,例如扩展卡尔曼滤波器 (EKF) 和无味卡 ...
基于自适应扩展卡尔曼滤波器（AEKF）的锂离子电池SOC估计（附MATLAB代码）
AEKF_SOC_Estimation函数使用二阶RC等效电路模型(ECM)和自适应扩展卡尔曼滤波器(AEKF)估计电池的端电压(Vt)和充电状态(SOC).该函数将以下内容作为输入: · 电流(A) ...
无人驾驶算法学习（三）：扩展卡尔曼滤波器Extended Kalman Filter
文章目录 1.引言 2. 扩展卡尔曼滤波数学理论 3. 代码实战 3.1 python实现 3.2结果分析 1.引言当系统状态方程不符合线性假设时,采用卡尔曼滤波无法获得理想的最优估计.高斯分布在非 ...
卡尔曼转矩观测_基于扩展卡尔曼滤波器的表贴式永磁同步电机负载转矩观测方法与流程...
本发明涉及永磁同步电机技术领域,具体涉及基于扩展卡尔曼滤波器的表贴式永磁同步电机负载转矩观测方法. 背景技术: 永磁同步电机(pmsm:permanentmagnetsynchronousmotor) ...
【目标定位】基于matlab扩展卡尔曼滤波器多机器人定位【含Matlab源码 2327期】
⛄一.简介 1 机器人运动模型分析 1.2 坐标系一个是全局坐标系(Xw,Yw), 另一个是机器人的局部坐标系(Xr,Yr). 如下图所示: 图1 机器人坐标系室内环境用二维平面表示在全局坐标系X ...

扩展卡尔曼滤波器设计

扩展卡尔曼滤波器设计相关推荐

最新文章

热门文章