建设网站报价,陕西省建设网三类人员官网,重庆公司网站seo,网站建设与管理专业学什么卡尔曼滤波器是利用系统状态方程#xff0c;结合测量结果对系统状态进行进行最优估计的算法。本文介绍它的主要公式#xff0c;并举例在C6000 DSP上实现。
推荐资料
KalmanFilter.NETUnderstanding Kalman Filters卡尔曼滤波与组合导航原理 “If you can’t explain it sim… 卡尔曼滤波器是利用系统状态方程结合测量结果对系统状态进行进行最优估计的算法。本文介绍它的主要公式并举例在C6000 DSP上实现。
推荐资料
KalmanFilter.NETUnderstanding Kalman Filters卡尔曼滤波与组合导航原理 “If you can’t explain it simply, you don’t understand it well enough.” —— Albert Einstein 第一个网站是我认为的讲卡尔曼滤波讲得最好的资料“如果你不能用简单的话把问题解释清楚那就是理解得不够”。作者就是能用例子把问题讲得很清楚的那种人。上面举的例子和介绍的思路都非常适合初学者即使是什么都不懂的人也能非常快速地理解。只是后面还有一些内容比如EKF、UKF等作者还没更新上去目前只有基础的卡尔曼滤波。 第二个链接是MathWorks给出的关于卡尔曼滤波的教学视频还有一个配套的关于单摆的实验可以一边动手操作一边学习感觉挺有意思的。 第三个链接是B站上的西北工业大学的严恭敏老师上课的录屏严老师还有一本书《捷联惯导算法与组合导航原理》相当于是课本推荐想要系统学习的同学去看这个。
基础 在正式开始介绍之前首先得有一些基础的概念。
随机变量的期望与方差 一维的随机变量xxx可以用它的期望μ\muμ和方差σ2\sigma^2σ2来描述多维的随机变量XXX可以用期望E(X){\rm E}(X)E(X)和协方差矩阵V(X){\rm V}(X)V(X)来描述。对于一个一维的随机变量xxx我们可以采样多个样本xix_ixi来对它的期望和方差进行估计。之所以说是“估计”是因为我们如果想获得真实的期望和方差就需要采样无穷多的样本这是不可能做到的。
E(x)1n∑i0n−1xiV(x)1n−1∑i0n−1(xi−μ)2{\rm{E}}(x) {1 \over n}\sum\limits_{i 0}^{n - 1} {{x_i}\;\;\;{\rm{V}}(x)} {1 \over {n - 1}}\sum\limits_{i 0}^{n - 1} {{{\left( {{x_i} - \mu } \right)}^2}} E(x)n1i0∑n−1xiV(x)n−11i0∑n−1(xi−μ)2 上面的是公式是利用nnn个样本对随机变量xxx的期望和方差的无偏估计。之所以说是无偏是因为如果再对上面的E(x){\rm E}(x)E(x)和V(x){\rm V}(x)V(x)求期望分别可以得到μ\muμ和σ2\sigma^2σ2它们是准确的估计。卡尔曼滤波算法中虽然不涉及这部分计算但是最好能有这样的概念方差可以用来衡量一个随机变量的变化程度。
期望代数Expectation Algebra 因为一维随机变量xxx是多维随机变量XXX的情况所以后面都以多维随机变量XXX进行讨论。为了和后面的推导统一符号也用xxx表示多维随机变量。期望代数是与随机变量的期望与方差相关的一些定理公式在卡尔曼滤波的推导中会用到。记xxx的均值和协方差矩阵分别为μx\mu_xμx和V(x){\rm V}(x)V(x)它们满足
V(x)E((x−μx)(X−μx)T){\rm{V}}(x) {\rm{E}}\left( {\left( {x - {\mu _x}} \right){{\left( {X - {\mu _x}} \right)}^{\rm T}}} \right)V(x)E((x−μx)(X−μx)T) 如果对随机变量xxx进行线性变换得到yFxyFxyFxFFF是N×NN\times NN×N的方阵xxx是NNN维列向量。那么随机变量yyy的方差为
V(y)FV(x)FT{\rm{V}}(y) F{\rm{V}}(x){F^{\rm T}}V(y)FV(x)FT 还有很多简单的公式这里就不列出来了都在这里可以找到这是后面推导的基础。
卡尔曼滤波 卡尔曼滤波可以概括为三个环节初始化更新和预测。
初始化 Initiate 初始化状态向量x^0,0\hat x_{0,0}x^0,0和表示当前状态估计不确定度的协方差矩阵P0,0P_{0,0}P0,0。 x^0,0\hat x_{0,0}x^0,0可以任意一般都初始化为0向量。在后续时刻经过多次测量后系统的状态向量能够收敛到真实值附近。
x^0,0[0,0,0,⋯,0]T{{\hat x}_{0,0}} {\left[ {0,0,0, \cdots ,0} \right]^{\rm{T}}}x^0,0[0,0,0,⋯,0]T 估计的状态x^\hat xx^的协方差矩阵称为估计不确定度Estimate Uncertainty用符号PPP表示。
Pn,nE((x^n,n−μxn,n)(x^n,n−μxn,n)T){P_{n,n}} {\rm{E}}\left( {\left( {{{\hat x}_{n,n}} - {\mu _{{x_{n,n}}}}} \right){{\left( {{{\hat x}_{n,n}} - {\mu _{{x_{n,n}}}}} \right)}^T}} \right)Pn,nE((x^n,n−μxn,n)(x^n,n−μxn,n)T) P0,0P_{0,0}P0,0表示在0时刻初始时刻对当前状态的估计不确定度。
P0,0[σ200⋯00σ20⋯000σ2⋯0⋮⋮⋮⋱00000σ2]{P_{0,0}} {\begin{bmatrix} {{\sigma ^2}}00 \cdots 0\\ 0{{\sigma ^2}}0 \cdots 0\\ 00{{\sigma ^2}} \cdots 0\\ \vdots \vdots \vdots \ddots 0\\ 0000{{\sigma ^2}} \end{bmatrix}} P0,0σ200⋮00σ20⋮000σ2⋮0⋯⋯⋯⋱00000σ2 x^0,0\hat x_{0,0}x^0,0 我们是随意设置的因此0时刻状态的真实期望值μx0,0{\mu_{{x_{0,0}}}}μx0,0很可能和我们随意设置的这个状态的估计值x^0,0\hat x_{0,0}x^0,0有很大的偏差。而这个估计的不确定度自然很大我们可以为每个维度的方差设置一个很大的值表示初始的估计是“不靠谱”的。同样随着后续的不断测量我们在每一时刻的估计不确定度Pn,nP_{n,n}Pn,n也会同系统状态向量的估计x^n,n\hat x_{n,n}x^n,n一起不断更新。 除了这x^0,0\hat x_{0,0}x^0,0和P0,0P_{0,0}P0,0需要初始化以外还有状态转移矩阵FFF系统噪声的协方差矩阵QQQ测量噪声的协方差矩阵RRR观测矩阵HHH需要给出。这几个矩阵都是常量不会随着时间变化在后面提到了再作介绍。
预测 Predict
系统状态预测 卡尔曼滤波的使用是建立在状态空间模型State Space Model之上的。状态外推方程state extrapolate equation用于状态的预测它的一般形式
x^n1,nFx^n,nGunwn{{\hat x}_{n 1,n}} F{{\hat x}_{n,n}} G{u_n} {w_n}x^n1,nFx^n,nGunwn
x^n1,n{{\hat x}_{n 1,n}}x^n1,n是在nnn时刻对下一时刻的状态的估计x^n,n{{\hat x}_{n,n}}x^n,n是在nnn时刻估计的nnn时刻的状态FFF是状态转移矩阵state transition matrixun{u_n}un是外部输入是确定的GGG是控制矩阵control matrix表示外部输入对系统状态的影响wn{w_n}wn是过程噪声process noise或者称为系统噪声无法直接测量。
估计不确定度预测
Pn,nE((x^n,n−μxn,n)(x^n,n−μxn,n)T){P_{n,n}} {\rm{E}}\left( {\left( {{{\hat x}_{n,n}} - {\mu _{{x_{n,n}}}}} \right){{\left( {{{\hat x}_{n,n}} - {\mu _{{x_{n,n}}}}} \right)}^T}} \right)Pn,nE((x^n,n−μxn,n)(x^n,n−μxn,n)T)
Pn1,nE((x^n1,n−μxn1,n)(x^n1,n−μxn1,n)T){P_{n 1,n}} {\rm{E}}\left( {\left( {{{\hat x}_{n 1,n}} - {\mu _{{x_{n 1,n}}}}} \right){{\left( {{{\hat x}_{n 1,n}} - {\mu _{{x_{n 1,n}}}}} \right)}^T}} \right)Pn1,nE((x^n1,n−μxn1,n)(x^n1,n−μxn1,n)T) 把状态转移方程代入上式后可以得到下式其中QQQ是过程噪声wnw_nwn的协方差矩阵。
Pn1,nFPn,nFTQ{P_{n 1,n}} F{P_{n,n}}{F^T} QPn1,nFPn,nFTQ 因为过程噪声wn{w_n}wn是无法直接测量的因此我们一般在初始化的时候就可以给出一个固定的过程噪声的协方差矩阵QQQ。QQQ矩阵里的值如果太小则会对系统状态的估计更加取信于模型的预测值而弱化了观测值的作用导致对系统状态的估计产生滞后误差lag error而如果QQQ矩阵里的值太大导致估计的不确定度增大则无法起到很好的滤波效果。
更新 Update
系统状态更新 首先是状态的更新从n−1n-1n−1时刻来到nnn时刻后多了一个观测值/测量值znz_nzn可以帮助我们更准确地进行估计。
x^n,nx^n,n−1Kn(zn−Hx^n,n−1){{\hat x}_{n,n}} {{\hat x}_{n,n - 1}} {K_n}\left( {{z_n} - H{{\hat x}_{n,n - 1}}} \right)x^n,nx^n,n−1Kn(zn−Hx^n,n−1) HHH是观测矩阵系统状态xxx中某些量我们可能是不能直接观测的而只有部分可以直接观测因此需要有一个观测矩阵HHH从状态向量中提出那些能够与观测值对应的部分。zn−Hx^n,n−1{z_n} - H{{\hat x}_{n,n - 1}}zn−Hx^n,n−1被记作innovation中文翻译为“新息”可以理解为“新的部分”。当前时刻的估计是上一时刻的估计与测量值的加权加权系数是卡尔曼增益KnK_nKn。卡尔曼增益的大小直接决定了新的估计值是更加侧重前一时刻的估计值还是测量值。
估计不确定度更新
Pn,n(I−KnH)Pn,n−1(I−KnH)TKnRnKnT{P_{n,n}} \left( {I - {K_n}H} \right){P_{n,n - 1}}{\left( {I - {K_n}H} \right)^{\rm{T}}} {K_n}{R_n}K_n^{\rm{T}}Pn,n(I−KnH)Pn,n−1(I−KnH)TKnRnKnT 估计不确定度的更新是根据状态更新的方程得到的可以重写一下状态更新方程
x^n,n(I−KnH)x^n,n−1Knzn{{\hat x}_{n,n}} \left( {I - K_nH} \right){{\hat x}_{n,n - 1}} {K_n}{z_n}x^n,n(I−KnH)x^n,n−1Knzn 再根据期望代数中的公式即可得到Pn,n{P_{n,n}}Pn,n的计算表达式。其中znz_nzn是测量值而RnR_nRn是测量噪声的协方差矩阵
znHxnvn{z_n} H{x_n} {v_n}znHxnvn xnx_nxn是nnn时刻系统真实的状态值也就是系统状态的期望值μxn,n{\mu _{{x_{n,n}}}}μxn,n, vnv_nvn是测量噪声RnE(vnvnT){R_n} {\rm{E}}\left( {{v_n}v_n^{\rm{T}}} \right)RnE(vnvnT)。详细的推导过程可以看这里。
卡尔曼增益计算 卡尔曼增益是令估计不确定度Pn,nP_{n,n}Pn,n的迹最小化得到的。Pn,nP_{n,n}Pn,n的对角线是系统状态xn,nx_{n,n}xn,n每一个维度的方差它的迹是这些方差之和。最小化tr(Pn,n)tr(P_{n,n})tr(Pn,n)可以通过对KnK_nKn求导并令导数为零矩阵然后得到最优的KnK_nKn。详细的推导过程可以看这里最后得到如下结果
KnPn,n−1HT(HPn,n−1HTRn)−1{K_n} {P_{n,n - 1}}{H^{\rm{T}}}{\left( {H{P_{n,n - 1}}{H^{\rm{T}}} {R_n}} \right)^{ - 1}}KnPn,n−1HT(HPn,n−1HTRn)−1 将这一结果再代入Pn,nP_{n,n}Pn,n的计算公式可以简化Pn,nP_{n,n}Pn,n的计算
Pn,n(I−KnH)Pn,n−1{P_{n,n}} \left( {I - {K_n}H} \right){P_{n,n - 1}}Pn,n(I−KnH)Pn,n−1
总结 图片引用自这里
举例 在C6000 DSP上实现二维坐标点(x,y)的卡尔曼滤波源代码。 首先实现了Matlab版本依次给出10个二维坐标点作为观测值然后将卡尔曼滤波后的结果输出显示DSP上采用相同的参数设置加载同样的10个二维坐标点将结果输出显示后与Matlab的结果进行对比确保结果正确后统计处理所需的时间。
匀速直线运动模型
state[x,vx,y,vy]state \left[ {x,{v_x},y,{v_y}} \right]state[x,vx,y,vy]
FCV[1100010000110001]{F_{CV}} {\begin{bmatrix} 1100\\ 0100\\ 0011\\ 0001 \end{bmatrix}} FCV1000110000100011 10次计算结果一致DSP上Kalman滤波器初始化需要6.2μs6.2{\rm{\mu s}}6.2μs每次更新和预测平均需要4.4μs4.4{\rm{\mu s}}4.4μs。
匀加速直线运动模型
state[x,vx,ax,y,vy,ay]state \left[ {x,{v_x},{a_x},y,{v_y},{a_y}} \right]state[x,vx,ax,y,vy,ay]
FCA[110.5000011000001000000110.5000011000001]{F_{CA}} {\begin{bmatrix} 11{0.5}000\\ 011000\\ 001000\\ 00011{0.5}\\ 000011\\ 000001 \end{bmatrix}}FCA1000001100000.5110000001000001100000.511 10次计算结果一致DSP上Kalman滤波器初始化需要7.0μs7.0{\rm{\mu s}}7.0μs每次更新和预测平均需要5.4μs5.4{\rm{\mu s}}5.4μs。
EKF UKF 如果系统状态的状态转移方程或者观测方程是非线性的那么就不能简单地用固定的FFF和HHH来表示系统的状态转移和观测矩阵。这时候就需要用到EKF或者UKF了。 EKF解决非线性的问题采用的是一阶泰勒展开来近似。这相比于KF就引入了额外的一些计算量因为FFF和HHH不是固定的每次系统状态改变都需要重新计算。 UKF采用的是类似蒙特卡洛分析的方法用几个中心对称的sigma点来估计非线性变换后的随机变量的协方差矩阵。
