江西省建设厅教育网站,最近新闻热点事件,长沙网站建设及推广公司,盐城市城乡建设局网站目录简介1.原理介绍场景假设(1).下一时刻的状态(2).增加系统的内部控制(3).考虑运动系统外部的影响(4).后验估计#xff1a;预测结果与观测结果的融合卡尔曼增益K2.卡尔曼滤波计算过程(1).预测阶段#xff08;先验估计阶段#xff09;(2).更新阶段#xff08;后验估计阶段预测结果与观测结果的融合卡尔曼增益K2.卡尔曼滤波计算过程(1).预测阶段先验估计阶段(2).更新阶段后验估计阶段3.代码举例简介
我们可以将卡尔曼滤波看做一种运动模型它可以在任何含有不确定信息的动态系统中对系统的下一步走向做出一定的预测无论是在单目标还是多目标领域都是很常用的一种算法。它最大的优点就是采用递归的方式来解决线性滤波的问题只需要当前的观测值和前一个周期的预测值就能够进行状态估计。
1.原理介绍
场景假设
假设有一个小车在路上行驶小车的初始位置和速度用一个状态向量表示 x^t[position,velocity]简写为x^t[pt,vt]\hat x_t[position,velocity]\\ 简写为\\ \hat x_t[p_t,v_t] x^t[position,velocity]简写为x^t[pt,vt] 因为用两个变量表示了状态向量所以系统中这两个变量的不确定性和相关性可以用一个协方差矩阵来表示 Pt[∑pp∑pv∑vp∑vv]P_t\begin{bmatrix} \sum_{pp}\quad\sum_{pv}\\ \sum_{vp}\quad\sum_{vv} \end{bmatrix} Pt[∑pp∑pv∑vp∑vv] 比如前一时刻的速度Vt−1V_{t-1}Vt−1和下一时刻的速度VtV_tVt存在着某种关联不可能从0突变成光速
比如前一时刻的速度Pt−1P_{t-1}Pt−1和下一时刻的速度PtP_tPt也存在着某种关联不可能从我国某地突然变成火星某地。
(1).下一时刻的状态
当小车在做匀速运动时小车下一时刻的位置和速度如下 {ptpt−1vt−1Δtvtvt−1\begin{cases} p_tp_{t-1}v_{t-1}\Delta t\\ v_tv_{t-1} \end{cases} {ptpt−1vt−1Δtvtvt−1
矩阵形式为 [ptvt][1Δt01][pt−1vt−1]Ft[pt−1vt−1]\begin{bmatrix} p_t\\ v_{t} \end{bmatrix} \begin{bmatrix} 1\quad\Delta t\\ 0\quad1 \end{bmatrix} \begin{bmatrix} p_{t-1}\\ v_{t-1} \end{bmatrix}F_t\begin{bmatrix} p_{t-1}\\ v_{t-1} \end{bmatrix}[ptvt][1Δt01][pt−1vt−1]Ft[pt−1vt−1]
即x^t[1Δt01]x^t−1Ftx^t−1\hat x_t\begin{bmatrix} 1\quad\Delta t\\ 0\quad1 \end{bmatrix}\hat x_{t-1}F_t\hat x_{t-1} x^t[1Δt01]x^t−1Ftx^t−1 记状态转移矩阵为表示小车状态随时间的变化: Ft[1Δt01]F_t\begin{bmatrix}1\quad\Delta t\\0\quad1\end{bmatrix} Ft[1Δt01] 系统的不确定性和相关性用PtP_tPt表示而下一时刻的协方差矩阵可表示为T表示矩阵的转置 PtFtPt−1FtTP_tF_tP_{t-1}F^T_t PtFtPt−1FtT
(2).增加系统的内部控制
如果对小车进行控制比如加速和减速假设它的加速度为ata_tat则小车下一时刻的位置和速度是
{ptpt−1vt−1Δt12atΔt2vtvt−1atΔt\begin{cases} p_tp_{t-1}v_{t-1}\Delta t\frac{1}{2}a_t\Delta t^2\\ v_tv_{t-1}a_t\Delta t \end{cases} {ptpt−1vt−1Δt21atΔt2vtvt−1atΔt 矩阵形式为 [ptvt][1Δt01][pt−1vt−1][Δt22Δt]atFt[pt−1vt−1]Btut\begin{bmatrix} p_t\\ v_{t} \end{bmatrix} \begin{bmatrix} 1\quad\Delta t\\ 0\quad1 \end{bmatrix} \begin{bmatrix} p_{t-1}\\ v_{t-1}\end{bmatrix}\begin{bmatrix}\frac{\Delta t^2}{2}\\\Delta t\end{bmatrix}a_t F_t\begin{bmatrix} p_{t-1}\\ v_{t-1} \end{bmatrix}B_t u_t [ptvt][1Δt01][pt−1vt−1][2Δt2Δt]atFt[pt−1vt−1]Btut 记状态控制矩阵为Bt[Δt22Δt]B_t\begin{bmatrix} \frac{\Delta t^2}{2}\\ \Delta t \end{bmatrix}Bt[2Δt2Δt]表明加速度如何改变小车的状态
记状态控制向量为 utu_tut表明控制的力度大小和方向。
(3).考虑运动系统外部的影响
小车运动时有很多因素可以对它的位置速度造成影响比如风俗路况等假设外部对小车造成的系统状态误差为wtw_twt并且它服从高斯分布wtN(0,Qt)w_t~N(0,Q_t)wt N(0,Qt)代入之前的小车运行控制方程可以得到最终完整的状态预测方程 x^tFtx^t−1BtutwtPtFtPt−1FtTQt\hat x_tF_t\hat x_{t-1}B_tu_tw_t\\ P_tF_tP_{t-1}F^T_tQ_t x^tFtx^t−1BtutwtPtFtPt−1FtTQt 除了小车运动的场景其他物体的状态变化
一般情况下假设wtw_twt为0即可将上述公式规范化得到 xFxBuPFPFTQxFxBu\\ PFPF^TQ xFxBuPFPFTQ
(4).后验估计预测结果与观测结果的融合
在等3步中我们预测出了小车的下一时刻的运动状态将它的结果称为预测结果也称为先验估计不过实际中汽车上会有里程表、GPS等设备也会提供小车下一时刻的运动状态称其结果为观测结果。
在实际情况中预测结果和观测结果都有一定的误差它们的结果都不一定十分准确而卡尔曼滤波最重要的作用就是将两者融合充分利用两者的不确定性得到更加准确的结果称之为后验估计。
卡尔曼增益K
融合阶段需要用到一个关键的系数卡尔曼增益K其公式如下推导过程略 KPHT(HPHT)RK\frac{PH^T}{(HPH^T)R} K(HPHT)RPHT P表示变量的不确定性和相关性的协方差矩阵
R表示传感器观测结果的噪声的协方差矩阵因为观测值的结果也不一定完美
H表示预测值和观测值之间的某种关系矩阵因为它们的结果存在的一定交合
整体来说卡尔曼增益表示对观测结果借鉴的程度相当于一个权重项让算法知道怎么利用观测与预测进行估计目的就是让最优估计值的方差更小是卡尔曼滤波的核心。
2.卡尔曼滤波计算过程
卡尔曼滤波的计算一般分为两步预测阶段和更新阶段。
(1).预测阶段先验估计阶段
xFxBuPFPFTQxFxBu\\ PFPF^TQ xFxBuPFPFTQ
(2).更新阶段后验估计阶段
KPHT((HPHT)R)−1xx(K(z−(Hx)))P(I−(KH))PKPH^T((HPH^T)R)^{-1}\\ xx(K(z-(Hx)))\\ P(I-(KH))P KPHT((HPHT)R)−1xx(K(z−(Hx)))P(I−(KH))P
x表示系统状态
z表示传感器的观测结果
F表示状态转移矩阵
B表示状态控制矩阵
u表示状态控制向量
H表示预测和观测的关系矩阵;
P表示变量的不确定性和相关性的协方差矩阵
R表示传感器观测结果的噪声的协方差矩阵
Q表示不确定因素对系统预测结果造成误差的协方差矩阵。
Q和R根据实际情况来定不断调优根据效果选择一个最优解
在实际计算中我们需要关注的是系统的状态x和它的误差协方差矩阵P卡尔曼增益K。
3.代码举例
