单片机控制卡尔曼滤波_单片机卡尔曼滤波 - CSDN
  • 卡尔曼滤波1

    千次阅读 2007-09-06 11:07:00
    卡尔曼滤波——做传感器想用的算法 只是不知道单片机是否跑得动……以下内容摘自维基百科卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器), 它能够从一系列的不完全包含噪声的测量(英文:measurement)中,估计动态...

     

    卡尔曼滤波——做传感器想用的算法

    只是不知道单片机是否跑得动……
    以下内容摘自维基百科

    卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器), 它能够从一系列的不完全包含噪声测量(英文:measurement)中,估计动态系统的状态。

    目录

    [隐藏]
    <script type="text/javascript"> // if (window.showTocToggle) { var tocShowText = "显示"; var tocHideText = "隐藏"; showTocToggle(); } // </script>

    [编辑] 应用实例

    卡尔曼滤波的一个典型实例是从一组有限的,包含噪声的,对物体位置的观察序列(可能有偏差)预测出物体的位置的坐标速度. 在很多工程应用(如雷达, 计算机视觉)中都可以找到它的身影. 同时,卡尔曼滤波也是控制理论以及控制系统工程中的一个重要课题.

    例如,对于雷达来说,人们感兴趣的是其能够跟踪目标.但目标的位置,速度,加速度的测量值往往在任何时候都有噪声.卡尔曼滤波利用目标的动态信息,设法去掉噪声的影响,得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计(滤波),也可以是对于将来位置的估计(预测),也可以是对过去位置的估计(插值或平滑).

    [编辑] 命名

    这种滤波方法以它的发明者鲁道夫.E.卡尔曼(Rudolph E. Kalman)命名,但是根据文献可知实际上Peter Swerling在更早之前就提出了一种类似的算法。

    斯坦利.施密特(Stanley Schmidt)首次实现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时,发现他的方法对于解决阿波罗计划的轨道预测很有用,后来阿波罗飞船的导航电脑便使用了这种滤波器。 关于这种滤波器的论文由Swerling (1958)、Kalman (1960)与 Kalman and Bucy (1961)发表。

    目前,卡尔曼滤波已经有很多不同的实现.卡尔曼最初提出的形式现在一般称为简单卡尔曼滤波器。除此以外,还有施密特扩展滤波器,信息滤波器以及很多Bierman, Thornton 开发的平方根滤波器的变种。也许最常见的卡尔曼滤波器是锁相环,它在收音机、计算机和几乎任何视频或通讯设备中广泛存在。


    以下的讨论需要线性代数以及概率论的一般知识。


    [编辑] 基本动态系统模型

    卡尔曼滤波建立在线性代数隐马尔可夫模型(hidden Markov model)上。其基本动态系统可以用一个马尔可夫链表示,该马尔可夫链建立在一个被高斯噪声(即正态分布的噪声)干扰的线性算子上的。系统的状态可以用一个元素为实数的向量表示。 随着离散时间的每一个增加,这个线性算子就会作用在当前状态上,产生一个新的状态,并也会带入一些噪声,同时系统的一些已知的控制器的控制信息也会被加入。同时,另一个受噪声干扰的线性算子产生出这些隐含状态的可见输出。

    为了从一系列有噪声的观察数据中用卡尔曼滤波器估计出被观察过程的内部状态,我们必须把这个过程在卡尔曼滤波的框架下建立模型。也就是说对于每一步k,定义矩阵Fk, Hk, Qk, Rk,有时也需要定义Bk,如下。

    卡尔曼滤波器的模型。圆圈代表向量,方块代表矩阵,星号代表高斯噪声,其协方差矩阵在右下方标出。
    卡尔曼滤波器的模型。圆圈代表向量,方块代表矩阵,星号代表高斯噪声,其协方差矩阵在右下方标出。

    卡尔曼滤波模型假设k时刻的真实状态是从(k − 1)时刻的状态演化而来,符合下式

    /textbf{x}_{k} = /textbf{F}_{k} /textbf{x}_{k-1} + /textbf{B}_{k}/textbf{u}_{k}  + /textbf{w}_{k}

    其中

    • Fk 是作用在xk−1上的状态变换模型(/矩阵/矢量)。
    • Bk 是作用在控制器向量uk上的输入-控制模型。
    • wk 是过程噪声,并假定其符合均值为零,协方差矩阵Qk多元正态分布
    /textbf{w}_{k} /sim N(0, /textbf{Q}_k)

    时刻k,对真实状态 xk的一个测量zk满足下式:

    /textbf{z}_{k} = /textbf{H}_{k} /textbf{x}_{k} + /textbf{v}_{k}

    其中Hk是观测模型,它把真实状态空间映射成观测空间,vk 是观测噪声,其均值为零,协方差为Rk,且服从正态分布

    /textbf{v}_{k} /sim N(0, /textbf{R}_k)

    初始状态以及每一时刻的噪声{x0, w1, ..., wk, v1 ... vk} 都为认为是互相独立的.

    实际上,很多真实世界的动态系统都并不确切的符合这个模型;但是由于卡尔曼滤波器被设计在有噪声的情况下工作,一个近似的符合已经可以使这个滤波器非常有用了。更多其它更复杂的卡尔曼滤波器的变种,在下边讨论中有描述。

    [编辑] 卡尔曼滤波器

    卡尔曼滤波是一种递归的估计,即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值,因此不需要记录观测或者估计的历史信息。卡尔曼滤波器与大多数滤波器不同之处,在于它是一种纯粹的时域滤波器,它不需要像低通滤波器频域滤波器那样,需要在频域设计再转换到时域实现。

    卡尔曼滤波器的状态由以下两个变量表示:

    • /hat{/textbf{x}}_{k|k},在时刻k 的状态的估计;
    • /textbf{P}_{k|k},误差相关矩阵,度量估计值的精确程度。

    卡尔曼滤波器的操作包括两个阶段:预测更新。在预测阶段,滤波器使用上一状态的估计,做出对当前状态的估计。在更新阶段,滤波器利用对当前状态的观测值优化在预测阶段获得的预测值,以获得一个更精确的新估计值。

    [编辑] 预测

    /hat{/textbf{x}}_{k|k-1} = /textbf{F}_{k}/hat{/textbf{x}}_{k-1|k-1} + /textbf{B}_{k} /textbf{u}_{k} (预测状态)
    /textbf{P}_{k|k-1} =  /textbf{F}_{k} /textbf{P}_{k-1|k-1} /textbf{F}_{k}^{T} + /textbf{Q}_{k} (预测估计协方差)

    [编辑] 更新

    /tilde{/textbf{y}}_{k} = /textbf{z}_{k} - /textbf{H}_{k}/hat{/textbf{x}}_{k|k-1} (测量革新或余量,英文分别为measurement innovation与residual,请见[1])
    /textbf{S}_{k} = /textbf{H}_{k}/textbf{P}_{k|k-1}/textbf{H}_{k}^{T} + /textbf{R}_{k} (测量革新(余量)协方差)
    /textbf{K}_{k} = /textbf{P}_{k|k-1}/textbf{H}_{k}^{T}/textbf{S}_{k}^{-1} (卡尔曼增益)
    /hat{/textbf{x}}_{k|k} = /hat{/textbf{x}}_{k|k-1} + /textbf{K}_{k}/tilde{/textbf{y}}_{k} (更新的状态估计)
    /textbf{P}_{k|k} = (I - /textbf{K}_{k} /textbf{H}_{k}) /textbf{P}_{k|k-1} (更新的协方差估计)

    使用上述公式计算/textbf{P}_{k|k}仅在最优卡尔曼增益的时候有效。使用其他增益的话,公式要复杂一些,请参见推导

    [编辑] 不变量(Invariant)

    如果模型准确,那么/hat{/textbf{x}}_{0|0}/textbf{P}_{0|0} 的值准确的反映了最初状态的分布,那么以下的不变量就得到了保存:所有估计的均值都为零

    • /textrm{E}[/textbf{x}_k - /hat{/textbf{x}}_{k|k}] = /textrm{E}[/textbf{x}_k - /hat{/textbf{x}}_{k|k-1}] = 0
    • /textrm{E}[/tilde{/textbf{y}}_k] = 0

    并且 协方差矩阵 准确的反映了以下估计的协方差:

    • /textbf{P}_{k|k} = /textrm{cov}(/textbf{x}_k - /hat{/textbf{x}}_{k|k})
    • /textbf{P}_{k|k-1} = /textrm{cov}(/textbf{x}_k - /hat{/textbf{x}}_{k|k-1})
    • /textbf{S}_{k} = /textrm{cov}(/tilde{/textbf{y}}_k)

    请注意,其中/textrm{E}[/textbf{a}] = 0, /textrm{cov}(/textbf{a}) = /textrm{E}[/textbf{a}/textbf{a}^T]

    [编辑] 实例

    考虑在无摩擦的、无限长的直轨道上的一辆车。该车最初停在位置 0 处,但时不时受到随机的冲击。我们每隔Δt秒即测量车的位置,但是这个测量是非精确的;我们想建立一个关于其位置以及速度的模型。我们来看如何推导出这个模型以及如何从这个模型得到卡尔曼滤波器。

    因为车上无动力,所以我们可以忽略掉Bkuk。由于FHRQ 是常数,所以时间下标可以去掉。

    车的位置以及速度(或者更加一般的,一个粒子的运动状态)可以被线性状态空间描述如下:

    /textbf{x}_{k} = /begin{bmatrix} x // /dot{x} /end{bmatrix}

    其中/dot{x}是速度,也就是位置对于时间的导数。

    我们假设在(k − 1)时刻与k时刻之间,车受到ak的加速度,其符合均值为0,标准差为σa正态分布。根据牛顿运动定律,我们可以推出

    /textbf{x}_{k} = /textbf{F} /textbf{x}_{k-1} + /textbf{G}a_{k}

    其中

    /textbf{F} = /begin{bmatrix} 1 & /Delta t // 0 & 1 /end{bmatrix}

    /textbf{G} = /begin{bmatrix} /frac{/Delta t^{2}}{2} // /Delta t /end{bmatrix}

    我们可以发现

    /textbf{Q} = /textrm{cov}(/textbf{G}a) = /textrm{E}[(/textbf{G}a)(/textbf{G}a)^{T}] = /textbf{G} /textrm{E}[a^2] /textbf{G}^{T} = /textbf{G}[/sigma_a^2]/textbf{G}^{T} = /sigma_a^2 /textbf{G}/textbf{G}^{T} (因为 σa 是一个标量).

    在每一时刻,我们对其位置进行测量,测量受到噪声干扰.我们假设噪声服从正态分布,均值为0,标准差为σz

    /textbf{z}_{k} = /textbf{H x}_{k} + /textbf{v}_{k}

    其中

    /textbf{H} = /begin{bmatrix} 1 & 0 /end{bmatrix}

    /textbf{R} = /textrm{E}[/textbf{v}_k /textbf{v}_k^{T}] = /begin{bmatrix} /sigma_z^2 /end{bmatrix}

    如果我们知道足够精确的车最初的位置,那么我们可以初始化

    /hat{/textbf{x}}_{0|0} = /begin{bmatrix} 0 // 0 /end{bmatrix}

    并且,我们告诉滤波器我们知道确切的初始位置,我们给出一个协方差矩阵:

    /textbf{P}_{0|0} = /begin{bmatrix} 0 & 0 // 0 & 0 /end{bmatrix}

    如果我们不确切的知道最初的位置与速度,那么协方差矩阵可以初始化为一个对角线元素是B的矩阵,B取一个合适的比较大的数。

    /textbf{P}_{0|0} = /begin{bmatrix} B & 0 // 0 & B /end{bmatrix}

    此时,与使用模型中已有信息相比,滤波器更倾向于使用初次测量值的信息。

    [编辑] 推导

    [编辑] 推导后验协方差矩阵

    按照上边的定义,我们从误差协方差Pk|k开始推导如下:

    /textbf{P}_{k|k}  = /textrm{cov}(/textbf{x}_{k} - /hat{/textbf{x}}_{k|k})

    代入/hat{/textbf{x}}_{k|k}

    /textbf{P}_{k|k} = /textrm{cov}(/textbf{x}_{k} - (/hat{/textbf{x}}_{k|k-1} + /textbf{K}_k/tilde{/textbf{y}}_{k}))

    再代入 /tilde{/textbf{y}}_k

    /textbf{P}_{k|k} = /textrm{cov}(/textbf{x}_{k} - (/hat{/textbf{x}}_{k|k-1} + /textbf{K}_k(/textbf{z}_k - /textbf{H}_k/hat{/textbf{x}}_{k|k-1})))

    /textbf{z}_{k}

    /textbf{P}_{k|k} = /textrm{cov}(/textbf{x}_{k} - (/hat{/textbf{x}}_{k|k-1} + /textbf{K}_k(/textbf{H}_k/textbf{x}_k + /textbf{v}_k - /textbf{H}_k/hat{/textbf{x}}_{k|k-1})))

    整理误差向量,得

    /textbf{P}_{k|k} = /textrm{cov}((I - /textbf{K}_k /textbf{H}_{k})(/textbf{x}_k - /hat{/textbf{x}}_{k|k-1}) - /textbf{K}_k /textbf{v}_k )

    因为测量误差vk 与其他项是非相关的,因此有

    /textbf{P}_{k|k} = /textrm{cov}((I - /textbf{K}_k /textbf{H}_{k})(/textbf{x}_k - /hat{/textbf{x}}_{k|k-1}))  + /textrm{cov}(/textbf{K}_k /textbf{v}_k )

    利用协方差矩阵的性质,此式可以写作

    /textbf{P}_{k|k} = (I - /textbf{K}_k /textbf{H}_{k})/textrm{cov}(/textbf{x}_k - /hat{/textbf{x}}_{k|k-1})(I - /textbf{K}_k /textbf{H}_{k})^{T}  + /textbf{K}_k/textrm{cov}(/textbf{v}_k )/textbf{K}_k^{T}

    使用不变量Pk|k-1以及Rk的定义 这一项可以写作 :/textbf{P}_{k|k} =  (I - /textbf{K}_k /textbf{H}_{k}) /textbf{P}_{k|k-1} (I - /textbf{K}_k /textbf{H}_{k})^T + /textbf{K}_k /textbf{R}_k /textbf{K}_k^T 这一公式对于任何卡尔曼增益Kk都成立。如果Kk是最优卡尔曼增益,则可以进一步简化,请见下文。


    [编辑] 最优卡尔曼增益的推导

    卡尔曼滤波器是一个最小均方误差估计器,后验状态误差估计(英文:posteriori state estimate)是

    /textbf{x}_{k} - /hat{/textbf{x}}_{k|k}

    我们最小化这个矢量幅度平方的期望值,/textrm{E}[|/textbf{x}_{k} - /hat{/textbf{x}}_{k|k}|^2],这等同于最小化后验估计协方差矩阵 Pk|k迹(trace)。将上面方程中的项展开、抵消,得到:

    /textbf{P}_{k|k} = /textbf{P}_{k|k-1} - /textbf{K}_k /textbf{H}_k /textbf{P}_{k|k-1} - /textbf{P}_{k|k-1} /textbf{H}_k^T /textbf{K}_k^T + /textbf{K}_k (/textbf{H}_k /textbf{P}_{k|k-1} /textbf{H}_k^T + /textbf{R}_k) /textbf{K}_k^T

    = /textbf{P}_{k|k-1} - /textbf{K}_k /textbf{H}_k /textbf{P}_{k|k-1} - /textbf{P}_{k|k-1} /textbf{H}_k^T /textbf{K}_k^T + /textbf{K}_k /textbf{S}_k/textbf{K}_k^T

    矩阵导数是 0 的时候得到迹(trace)的最小值:

    /frac{d /; /textrm{tr}(/textbf{P}_{k|k})}{d /;/textbf{K}_k} = -2 (/textbf{H}_k /textbf{P}_{k|k-1})^T + 2 /textbf{K}_k /textbf{S}_k  = 0

    从这个方程解出卡尔曼增益Kk

    /textbf{K}_k /textbf{S}_k = (/textbf{H}_k /textbf{P}_{k|k-1})^T = /textbf{P}_{k|k-1} /textbf{H}_k^T
    /textbf{K}_{k} = /textbf{P}_{k|k-1} /textbf{H}_k^T /textbf{S}_k^{-1}

    这个增益称为最优卡尔曼增益,在使用时得到最小均方误差

    [编辑] 后验误差协方差公式的化简

    在卡尔曼增益等于上面导出的最优值时,计算后验协方差的公式可以进行简化。在卡尔曼增益公式两侧的右边都乘以 SkKkT 得到

    /textbf{K}_k /textbf{S}_k /textbf{K}_k^T = /textbf{P}_{k|k-1} /textbf{H}_k^T /textbf{K}_k^T

    根据上面后验误差协方差展开公式,

    /textbf{P}_{k|k} = /textbf{P}_{k|k-1} - /textbf{K}_k /textbf{H}_k /textbf{P}_{k|k-1}  - /textbf{P}_{k|k-1} /textbf{H}_k^T /textbf{K}_k^T + /textbf{K}_k /textbf{S}_k /textbf{K}_k^T

    最后两项可以抵消,得到

    /textbf{P}_{k|k} = /textbf{P}_{k|k-1} - /textbf{K}_k /textbf{H}_k /textbf{P}_{k|k-1} = (I - /textbf{K}_{k} /textbf{H}_{k}) /textbf{P}_{k|k-1}.

    这个公式的计算比较简单,所以实际中总是使用这个公式,但是需注意这公式仅在使用最优卡尔曼增益的时候它才成立。如果算术精度总是很低而导致numerical stability出现问题,或者特意使用非最优卡尔曼增益,那么就不能使用这个简化;必须使用上面导出的后验误差协方差公式。

    [编辑] 与递归Bayesian估计之间的关系

    假设真正的状态是无法观察的马尔科夫过程,测量结果是从隐性马尔科夫模型观察到的状态。

    Image:HMMKalmanFilterDerivation.png

    根据马尔科夫假设,真正的状态仅受最近一个状态影响而与其它以前状态无关。

    p(/textbf{x}_k|/textbf{x}_0,/dots,/textbf{x}_{k-1}) = p(/textbf{x}_k|/textbf{x}_{k-1})

    与此类似,在时刻 k 测量只与当前状态有关而与其它状态无关。

    p(/textbf{z}_k|/textbf{x}_0,/dots,/textbf{x}_{k}) = p(/textbf{z}_k|/textbf{x}_{k} )

    根据这些假设,隐性马尔科夫模型所有状态的概率分布可以简化为:

    p(/textbf{x}_0,/dots,/textbf{x}_k,/textbf{z}_1,/dots,/textbf{z}_k) = p(/textbf{x}_0)/prod_{i=1}^k p(/textbf{z}_i|/textbf{x}_i)p(/textbf{x}_i|/textbf{x}_{i-1})

    However, when the Kalman filter is used to estimate the state x, the probability distribution of interest is that associated with the current states conditioned on the measurements up to the current timestep. (This is achieved by marginalizing out the previous states and dividing by the probability of the measurement set.) 但是,当卡尔曼滤波器用来估计状态x的时候,

    This leads to the predict and update steps of the Kalman filter written probabilistically. The probability distribution associated with the predicted state is product of the probability distribution associated with the transition from the (k − 1)th timestep to the kth and the probability distribution associated with the previous state, with the true state at (k − 1) integrated out.

    p(/textbf{x}_k|/textbf{Z}_{k-1}) = /int p(/textbf{x}_k | /textbf{x}_{k-1}) p(/textbf{x}_{k-1} | /textbf{Z}_{k-1} )  /, d/textbf{x}_{k-1}

    The measurement set up to time t is

    /textbf{Z}_{t} = /left /{ /textbf{z}_{1},/dots,/textbf{z}_{t} /right /}

    The probability distribution of updated is proportional to the product of the measurement likelihood and the predicted state.

    p(/textbf{x}_k|/textbf{Z}_{k}) = /frac{p(/textbf{z}_k|/textbf{x}_k) p(/textbf{x}_k|/textbf{Z}_{k-1})}{p(/textbf{z}_k|/textbf{Z}_{k-1})}

    The denominator

    p(/textbf{z}_k|/textbf{Z}_{k-1}) = /int p(/textbf{z}_k|/textbf{x}_k) p(/textbf{x}_k|/textbf{Z}_{k-1}) d/textbf{x}_k

    is an unimportant normalization term.

    The remaining probability density functions are

    p(/textbf{x}_k | /textbf{x}_{k-1}) = N(/textbf{x}_k, /textbf{F}_k/textbf{x}_{k-1}, /textbf{Q}_k)
    p(/textbf{z}_k|/textbf{x}_k) = N(/textbf{z}_k,/textbf{H}_{k}/textbf{x}_k, /textbf{R}_k)
    p(/textbf{x}_{k-1}|/textbf{Z}_{k-1}) = N(/textbf{x}_{k-1},/hat{/textbf{x}}_{k-1},/textbf{P}_{k-1} )

    Note that the PDF at the previous timestep is inductively assumed to be the estimated state and covariance. This is justified because, as an optimal estimator, the Kalman filter makes best use of the measurements, therefore the PDF for /mathbf{x}_k given the measurements /mathbf{Z}_kis the Kalman filter estimate.

    [编辑] 信息滤波器

    In the information filter, or inverse covariance filter, the estimated covariance and estimated state are replaced by the information matrix and information vector respectively.

    /textbf{Y}_{k|k} /equiv  /textbf{P}_{k|k}^{-1}
    /hat{/textbf{y}}_{k|k} /equiv  /textbf{P}_{k|k}^{-1}/hat{/textbf{x}}_{k|k}

    Similarly the predicted covariance and state have equivalent information forms,

    /textbf{Y}_{k|k-1} /equiv  /textbf{P}_{k|k-1}^{-1}
    /hat{/textbf{y}}_{k|k-1} /equiv  /textbf{P}_{k|k-1}^{-1}/hat{/textbf{x}}_{k|k-1}

    as have the measurement covariance and measurement vector.

    /textbf{I}_{k} /equiv /textbf{H}_{k}^{T} /textbf{R}_{k}^{-1} /textbf{H}_{k}
    /textbf{i}_{k} /equiv /textbf{H}_{k}^{T} /textbf{R}_{k}^{-1} /textbf{z}_{k}

    The information update now becomes a trivial sum.

    /textbf{Y}_{k|k} = /textbf{Y}_{k|k-1} + /textbf{I}_{k}
    /hat{/textbf{y}}_{k|k} = /hat{/textbf{y}}_{k|k-1} + /textbf{i}_{k}

    The main advantage of the information filter is that N measurements can be filtered at each timestep simply by summing their information matrices and vectors.

    /textbf{Y}_{k|k} = /textbf{Y}_{k|k-1} + /sum_{j=1}^N /textbf{I}_{k,j}
    /hat{/textbf{y}}_{k|k} = /hat{/textbf{y}}_{k|k-1} + /sum_{j=1}^N /textbf{i}_{k,j}

    To predict the information filter the information matrix and vector can be converted back to their state space equivalents, or alternatively the information space prediction can be used.

    /textbf{M}_{k} = [/textbf{F}_{k}^{-1}]^{T} /textbf{Y}_{k|k} /textbf{F}_{k}^{-1}
    /textbf{C}_{k} = /textbf{M}_{k} [/textbf{M}_{k}+/textbf{Q}_{k}^{-1}]^{-1}
    /textbf{L}_{k} = I - /textbf{C}_{k}
    /textbf{Y}_{k|k-1} = /textbf{L}_{k} /textbf{M}_{k} /textbf{L}_{k}^{T} +                                    /textbf{C}_{k} /textbf{Q}_{k}^{-1} /textbf{C}_{k}^{T}
    /hat{/textbf{y}}_{k|k-1} = /textbf{L}_{k} [/textbf{F}_{k}^{-1}]^{T}/hat{/textbf{y}}_{k|k}

    Note that if F and Q are time invariant these values can be cached. Note also that F and Q need to be invertible.

    [编辑] 非线性滤波器

    基本卡尔曼滤波器(The basic Kalman filter)是限制在线性的假设之下。然而,大部份非平凡的(non-trial)的系统都是非线性系统。其中的“非线性性质”(non- linearity )可能是伴随存在过程模型(process model)中或观测模型(observation model)中,或者两者兼有之。

    [编辑] 扩展卡尔曼滤波器

    在扩展卡尔曼滤波器(EKF)中状态转换和观测模型不需要是状态的线性函数,可替换为(可微的)函数。

    /textbf{x}_{k} = f(/textbf{x}_{k-1}, /textbf{u}_{k}, /textbf{w}_{k})
    /textbf{z}_{k} = h(/textbf{x}_{k}, /textbf{v}_{k})

    函数 f 可以用来从过去的估计值中计算预测的状态,相似的,函数 h可以用来以预测的状态计算预测的测量值。然而 fh 不能直接的应用在协方差中,取而代之的是计算偏导矩阵(Jacobian)。

    在每一步中使用当前的估计状态计算Jacobian矩阵,这几个矩阵可以用在卡尔曼滤波器的方程中。这个过程,实质上将非线性的函数在当前估计值处线性化了。

    这样一来,卡尔曼滤波器的等式为:

    预测

    /hat{/textbf{x}}_{k|k-1} = f(/textbf{x}_{k-1}, /textbf{u}_{k}, 0)
    /textbf{P}_{k|k-1} =  /textbf{F}_{k} /textbf{P}_{k-1|k-1} /textbf{F}_{k}^{T} + /textbf{Q}_{k}

    使用Jacobians矩阵更新模型

    /textbf{F}_{k} = /left . /frac{/partial f}{/partial /textbf{x} } /right /vert _{/hat{/textbf{x}}_{k-1|k-1},/textbf{u}_{k}}
    /textbf{H}_{k} = /left . /frac{/partial h}{/partial /textbf{x} } /right /vert _{/hat{/textbf{x}}_{k|k-1}}

    更新

    /tilde{/textbf{y}}_{k} = /textbf{z}_{k} - h(/hat{/textbf{x}}_{k|k-1}, 0)
    /textbf{S}_{k} = /textbf{H}_{k}/textbf{P}_{k|k-1}/textbf{H}_{k}^{T} + /textbf{R}_{k}
    /textbf{K}_{k} = /textbf{P}_{k|k-1}/textbf{H}_{k}^{T}/textbf{S}_{k}^{-1}
    /hat{/textbf{x}}_{k|k} = /hat{/textbf{x}}_{k|k-1} + /textbf{K}_{k}/tilde{/textbf{y}}_{k}
    /textbf{P}_{k|k} = (I - /textbf{K}_{k} /textbf{H}_{k}) /textbf{P}_{k|k-1}

    [编辑] Unscented Kalman filter

    The extended Kalman filter gives particularly poor performance on highly non-linear functions because only the mean is propagated through the non-linearity. The unscented Kalman filter (UKF) [JU97] uses a deterministic sampling technique to pick a minimal set of sample points (called sigma points) around the mean. These sigma points are then propagated through the non-linear functions and the covariance of the estimate is then recovered. The result is a filter which more accurately captures the true mean and covariance. (This can be verified using Monte Carlo sampling or through a Taylor series expansion of the posterior statistics.) In addition, this technique removes the requirement to analytically calculate Jacobians, which for complex functions can be a difficult task in itself.

    预测

    如同扩展卡尔曼滤波器(EKF)一样, UKF的预测过程可以独立于UKF的更新过程之外,与一个线性的(或者确实是扩展卡尔曼滤波器的)更新过程合并来使用;或者,UKF的预测过程与更新过程在上述中地位互换亦可。

    The estimated state and covariance are augmented with the mean and covariance of the process noise.

    /textbf{x}_{k-1|k-1}^{a} = [ /hat{/textbf{x}}_{k-1|k-1}^{T} /quad E[/textbf{w}_{k}^{T}] / ]^{T}
    /textbf{P}_{k-1|k-1}^{a} = /begin{bmatrix} & /textbf{P}_{k-1|k-1} & & 0 & // & 0 & &/textbf{Q}_{k} & /end{bmatrix}

    A set of 2L+1 sigma points is derived from the augmented state and covariance where L is the dimension of the augmented state.

    /chi_{k-1|k-1}^{0} = /textbf{x}_{k-1|k-1}^{a}
    /chi_{k-1|k-1}^{i} =/textbf{x}_{k-1|k-1}^{a} + /left ( /sqrt{ (L + /lambda) /textbf{P}_{k-1|k-1}^{a} } /right )_{i} i = 1..L /,/!
    /chi_{k-1|k-1}^{i} = /textbf{x}_{k-1|k-1}^{a} - /left ( /sqrt{ (L + /lambda) /textbf{P}_{k-1|k-1}^{a} } /right )_{i-L} i = L+1,/dots{}2L /,/!

    The sigma points are propagated through the transition function f.

    /chi_{k|k-1}^{i} = f(/chi_{k-1|k-1}^{i}) /quad i = 0..2L

    The weighted sigma points are recombined to produce the predicted state and covariance.

    /hat{/textbf{x}}_{k|k-1} = /sum_{i=1}^N W_{s}^{i} /chi_{k|k-1}^{i}
    /textbf{P}_{k|k-1} = /sum_{i=1}^N W_{c}^{i}/ [/chi_{k|k-1}^{i} - /hat{/textbf{x}}_{k|k-1}] [/chi_{k|k-1}^{i} - /hat{/textbf{x}}_{k|k-1}]^{T}

    Where the weights for the state and covariance are given are:

    W_{s}^{0} = /frac{/lambda}{L+/lambda}
    W_{c}^{0} = /frac{/lambda}{L+/lambda} + (1 - /alpha^2 + /beta)
    W_{s}^{i} = W_{c}^{i} = /frac{1}{2(L+/lambda)}
    /lambda = /alpha^2 (L+/kappa) - L /,/!

    Typical values for α, β, and κ are 10 − 3, 2 and 0 respectively. (These values should suffice for most purposes.)

    更新

    The predicted state and covariance are augmented as before, except now with the mean and covariance of the measurement noise.

    /textbf{x}_{k|k-1}^{a} = [ /hat{/textbf{x}}_{k|k-1}^{T} /quad E[/textbf{v}_{k}^{T}] / ]^{T}
    /textbf{P}_{k|k-1}^{a} = /begin{bmatrix} & /textbf{P}_{k|k-1} & & 0 & // & 0 & &/textbf{R}_{k} & /end{bmatrix}

    As before, a set of 2L + 1 sigma points is derived from the augmented state and covariance where L is the dimension of the augmented state.

    /chi_{k|k-1}^{0} = /textbf{x}_{k|k-1}^{a}
    /chi_{k|k-1}^{i} =/textbf{x}_{k|k-1}^{a} + /left ( /sqrt{ (L + /lambda) /textbf{P}_{k|k-1}^{a} } /right )_{i} i = 1..L /,/!
    /chi_{k|k-1}^{i} = /textbf{x}_{k|k-1}^{a} - /left ( /sqrt{ (L + /lambda) /textbf{P}_{k|k-1}^{a} } /right )_{i-L} i = L+1,/dots{}2L /,/!

    Alternatively if the UKF prediction has been used the sigma points themselves can be augmented along the following lines

    /chi_{k|k-1} := [ /chi_{k|k-1} /quad E[/textbf{v}_{k}^{T}] / ]^{T} /pm /sqrt{ (L + /lambda) /textbf{R}_{k}^{a} }

    where

    /textbf{R}_{k}^{a} = /begin{bmatrix} & 0 & & 0 & // & 0 & &/textbf{R}_{k} & /end{bmatrix}

    The sigma points are projected through the observation function h.

    /gamma_{k}^{i} = h(/chi_{k|k-1}^{i}) /quad i = 0..2L

    The weighted sigma points are recombined to produce the predicted measurement and predicted measurement covariance.

    /hat{/textbf{z}}_{k} = /sum_{i=1}^N W_{s}^{i} /gamma_{k}^{i}
    /textbf{P}_{z_{k}z_{k}} = /sum_{i=1}^N W_{c}^{i}/ [/gamma_{k}^{i} - /hat{/textbf{z}}_{k}] [/gamma_{k}^{i} - /hat{/textbf{z}}_{k}]^{T}

    The state-measurement cross-correlation matrix,

    /textbf{P}_{x_{k}z_{k}} = /sum_{i=1}^N W_{c}^{i}/ [/chi_{k|k-1}^{i} - /hat{/textbf{x}}_{k|k-1}] [/gamma_{k}^{i} - /hat{/textbf{z}}_{k}]^{T}

    is used to compute the UKF Kalman gain.

    K_{k} = /textbf{P}_{x_{k}z_{k}} /textbf{P}_{z_{k}z_{k}}^{-1}

    As with the Kalman filter, the updated state is the predicted state plus the innovation weighted by the Kalman gain,

    /hat{/textbf{x}}_{k|k} = /hat{/textbf{x}}_{k|k-1} + K_{k}( /textbf{z}_{k} - /hat{/textbf{z}}_{k} )

    And the updated covariance is the predicted covariance, minus the predicted measurement covariance, weighted by the Kalman gain.

    /textbf{P}_{k|k} = /textbf{P}_{k|k} - K_{k} /textbf{P}_{z_{k}z_{k}} K_{k}^{T}

    [编辑] 应用

    [编辑] 参见

    [编辑] 外部连接

    [编辑] 参考文献

    • Gelb A., editor. Applied optimal estimation. MIT Press, 1974.
    • Kalman, R. E. A New Approach to Linear Filtering and Prediction Problems, Transactions of the ASME - Journal of Basic Engineering Vol. 82: pp. 35-45 (1960)
    • Kalman, R. E., Bucy R. S., New Results in Linear Filtering and Prediction Theory, Transactions of the ASME - Journal of Basic Engineering Vol. 83: pp. 95-107 (1961)
    • [JU97] Julier, Simon J. and Jeffery K. Uhlmann. A New Extension of the Kalman Filter to nonlinear Systems. In The Proceedings of AeroSense: The 11th International Symposium on Aerospace/Defense Sensing,Simulation and Controls, Multi Sensor Fusion, Tracking and Resource Management II, SPIE, 1997.
    • Harvey, A.C. Forecasting, Structural Time Series Models and the Kalman Filter. Cambridge University Press, Cambridge, 1989 
    展开全文
  • 单片机ADC采样算法----卡尔曼滤波

    千次阅读 2020-03-28 14:36:17
    关于卡尔曼滤波,看看百度百科上的定义 算法的核心思想是,根据当前的仪器"测量值" 和上一刻的 "预测量" 和 "误差",计算得到当前的最优量. 再预测下一刻的量,里面比较突出的是观点是.把误差纳入计算, 而且分为...

    关于卡尔曼滤波,看看百度百科上的定义

          算法的核心思想是,根据当前的仪器"测量值" 和上一刻的 "预测量" 和 "误差",计算得到当前的最优量.   再 预测下一刻的量, 里面比较突出的是观点是. 把误差纳入计算, 而且分为预测误差和测量误差两种.通称为 噪声. 还有一个非常大的特点是,误差独立存在, 始终不受测量数据的影响

    下来先了解一个卡尔曼滤波中几个参数的含义:概率(Probability),随即变量(Random Variable),高斯或正态分配(Gaussian Distribution)还有State-space Model等等。

    关于卡尔曼公式的含义及推导,网上已经有很多文章了,这里不在赘述,直接看C代码的实现。

    /*
        R值固定,Q值越大,代表越信任测量值,Q值无穷大,代表只用测量值。
                 Q值越小,代表越信任模型预测值,Q值为0,代表只用模型预测值。
    */
    //参数一
    float  KalmanFilter( float  inData )
    {
        static float prevData = 0;                                 //上一个数据
        static float p = 10, q = 0.001, r = 0.001, kGain = 0;      // q 控制误差 r 控制响应速度
        p = p + q;
        kGain = p / ( p + r );                                      //计算卡尔曼增益
        inData = prevData + ( kGain * ( inData - prevData ) );      //计算本次滤波估计值
        p = ( 1 - kGain ) * p;                                      //更新测量方差
        prevData = inData;
        return inData;                                             //返回估计值
    }

    现在测试一下卡尔曼滤波的效果,通过函数发生器产生一个锯齿波,送到单片机的AD口,单片机读取采集到的AD数据后,经过卡尔曼滤波算法,然后将采样的数据和滤波后的数据通过串口发生出来,并在串口波形显示软件上显示。

    void main( void )
    {
      
        while( 1 )
        {
            val1 = ReadVol_CH3();            //读取AD采样数据
            dat = ( float )val1;
            dat =    KalmanFilter( dat );    //卡尔曼滤波
            printf("A%d\r\n",val1);          //打印结果
            printf("B%2f\r\n",dat);
        }
    }

    现在看一下滤波的结果

    蓝色曲线为原始采样的数据曲线,橙色曲线为经过卡尔曼滤波后的曲线。

    下面改变Q和R的值在测试一下滤波效果。

    修改后的参数如下

    //参数二
    unsigned long kalman_filter( unsigned long ADC_Value )
    {
        float LastData;
        float NowData;
        float kalman_adc;
        static float kalman_adc_old = 0;
        static float P1;
        static float Q = 0.0003;
        static float R = 5;
        static float Kg = 0;
        static float P = 1;
        NowData = ADC_Value;
        LastData = kalman_adc_old;
        P = P1 + Q;
        Kg = P / ( P + R );
        kalman_adc = LastData + Kg * ( NowData - kalman_adc_old );
        P1 = ( 1 - Kg ) * P;
        P = P1;
        kalman_adc_old = kalman_adc;
        return ( unsigned long )( kalman_adc );
    }

    测试波形

    蓝色曲线为原始采样的数据曲线,橙色曲线为经过卡尔曼滤波后的曲线。

    和第一次测试的波形图对比后可以发现,第二次经过卡尔曼滤波后的波形变化非常大,参数改变后锯齿波被滤成接近于直线了。

    可以看到不同的R、Q值会对测量结果有很大的影响。

    Q:过程噪声,Q增大,动态响应变快,收敛稳定性变坏

    R:测量噪声,R增大,动态响应变慢,收敛稳定性变好

    具体各个参数的如何选择,只有在应用中根据测量结果,自己慢慢调整。目前还没有找到权威的资料来讲解这些参数如何选择。

     

     

    展开全文
  • 卡尔曼滤波在温度测量上的应用

    千次阅读 2019-09-18 19:20:30
    卡尔曼滤波的应用简介卡尔曼滤波算法卡尔曼滤波算法对温度的估计 简介 对于卡尔曼滤波,我相信很多机器人相关专业的同学都不陌生,网上搜索下关键词相关的资料也很多,但是大部分都是给你介绍相关的公式推导,对于...

    简介

    对于卡尔曼滤波,我相信很多机器人相关专业的同学都不陌生,网上搜索下关键词相关的资料也很多,但是大部分都是给你介绍相关的公式推导,对于数学基础好点的同学来说看了遍之后会觉得好像是那么回事,但是回头一想会觉得这东西怎么用啊,如何把它的效果在实际工程应用中发挥出来啊,如果做不到这一点那么对于你来说卡尔曼滤波还是遥不可及的梦。其实我觉得卡尔曼滤波这个名字是具有迷惑性的,以至于我们会把他归类于低通滤波,高通滤波这类的滤波器当中,在学习了相关理论后你会发现卡尔曼滤波并不是一般的滤波器而是一种“”“预测”,根据当前时刻的测量值和上一时刻的估计值预测当前时刻的最优值,所以这里我们就可以看出来卡尔曼滤波不是简单地滤除某些高频或者低频噪声而是对真实值的预测。

    卡尔曼滤波算法

    1960年,Kalman首次提出卡尔曼滤波,卡尔曼滤波刚一提出人们就发现了其在军事上的巨大应用前景,阿波罗登月、飞机导弹的导航系统成功应用就是最好的例证。,卡尔曼滤波器是时域内直接设计的最优滤波器,1961 年卡尔曼有将这一滤波理论推广到连续时间系统中,也就形成了卡尔曼滤波的完整体系,对于系统噪声是高斯分布的线性系统可以递推出最小均方差估计。
    从本质上来讲卡尔曼滤波其实就是反馈控制,其实现的思路如下图所示。滤波器估计过程中某一个时刻的状态,利用量测更新的值作为反馈。所以卡尔曼滤波过程又可以分为状态跟新和测量更新两个部分来实现。状更新中主要是为了获得下个时刻的先验估计,量测更新则是为了通过先验估计和量测值获取后验估计。这就是卡尔曼思想中采用的迭代方式。具体的过程如下
    状态一步预测:
    x^kk1=Φkk1x^k1\hat{x}_{k | k-1}=\Phi_{k | k-1} \hat{x}_{k-1}
    状态估计:
    x^k=x^kk1+Kk(zkHkx^kk1)\hat{x}_{k}=\hat{x}_{k| k-1}+K_{k}\left(z_{k}-H_{k} \hat{x}_{k| k-1}\right)
    计算滤波增益:
    Kk=Pk,k1HkT(HkPkk1HkT+Rk)1K_{k}=P_{k, k-1} H_{k}^{T}\left(H_{k} P_{k|k-1} H_{k}^{T}+R_{k}\right)^{-1}
    一步预测方差矩阵:
    Pkk1=Φkk1Pk1Φkk1+Γkk1Qk1Γkk1TP_{k | k-1}=\Phi_{k | k-1} P_{k-1} \Phi_{k| k-1}+\Gamma_{k |k-1} Q_{k-1} \Gamma_{k |k-1}^{T}
    估计误差方差阵:
    Pk=(IKkHk)Pkk1(IKkHk)+KkRkKkTP_{k}=\left(I-K_{k} H_{k}\right) P_{k| k-1}\left(I-K_{k} H_{k}\right)+K_{k} R_{k} K_{k}^{T}

    具体的推导过程就不表了,相信大家在网上都能找到,从这五条公式中我们可以看到一下几个要点,第一个就是初始状态的选取,第二个就是噪声矩阵大小的选取。对于第一个问题需要根据实际情况来判断,比如说对于加速速计,一般来说它的初始值是Z轴方向为重力加速度值g,其他两个轴为0,而对于温度传感器,由于在不同环境下它的初始值都不一样所以我们可以选择第一个测量值作为初始值,但是无论怎么选择其实是不会影响最终的结果的只会影响到收敛速度。而对于第二个问题我之前也是查阅了大量的资料,发现很多人其实没有直面这个问题,一种比较靠谱的方法是将实时运行的数据保存下来导入到matlab中,然后在matlab中调试出比较好的Q,R参数。下面举一个例子

    卡尔曼滤波算法对温度的估计

    对温度系统的估计是卡尔曼应用中最常使用的案例,我们首先采集一段温度传感器读取的室内温度,然后在matlab中进行计算,matlab代码如下

    N=1000; 
    size=[N,1]; 
    
    %取温度预测值的方差为Q=1e-2,温度传感器的测量方差为R=0.36,即我们更相信预测值,而较少相信传感器测量值。
    Q=0.001;  
    R=0.36;
    a=0.1;
    % T_mearsured=T+sqrt(R)*randn(size);
    T_mearsured=load('temperature_data.txt');
    %初始时刻温度的最优估计值为T_start=22.5,温度初始估计方差为P_start=2
    lowpass_filter(1)=T_mearsured(1);
    T_start=T_mearsured(1);  P_start=2;
    T_kalman(1)=T_start;  P_kalman(1)=P_start;
    %用_kalman的后缀表示最优估计值,用_pre的后缀表示预测值
    for k=2:N
    %在进行温度预测时,因为温度是一个连续的状态,我们认为上一时刻的温度和当前时刻的温度相等,则有T(k)=T(k-1)T_pre(k)=T_kalman(k-1);
    P_pre(k)=P_kalman(k-1)+Q;
    K(k)=P_pre(k)/(P_pre(k)+R);
    T_kalman(k)=T_pre(k)+K(k)*(T_mearsured(k)-T_pre(k));
    P_kalman(k)=P_pre(k)-K(k)*P_pre(k);
    end
    %画图
    figure();
    plot(T_mearsured,'-k');
    hold on
    plot(T_kalman,'r');
    legend('温度测量值','Kalman估计值')
    

    运行结果如下
    在这里插入图片描述
    黑色是我们实际测量的数据,红色是经过卡尔曼滤波算法后的数值,可以看到滤波后的效果还是很明显的,这里可以看到我设置的Q、R值分别是0.01和0.36我们改一下这个值看看会有啥不同,当我们把Q的值改为0.5时可以看到效果为
    在这里插入图片描述
    两条线的重合度更高,这也是符合理论的,因为增大了系统噪声,说明我们更信赖测量值,与测量值关系就更密切。
    对应的C语言代码为

    *-------------------------------------------------------------------------------------------------------------*/
    /*       
            Q:过程噪声,Q增大,动态响应变快,收敛稳定性变坏
            R:测量噪声,R增大,动态响应变慢,收敛稳定性变好       
    */
    
    #define KALMAN_Q 0.02
    
    #define KALMAN_R 7.0000
    
    /* 卡尔曼滤波处理 */
    
    static double KalmanFilter(const double ResrcData,double ProcessNiose_Q,double MeasureNoise_R)
    {
    
        double R = MeasureNoise_R;
        double Q = ProcessNiose_Q;
    
        static double x_last;
        double x_mid = x_last;
        double x_now;
    
        static double p_last;
        double p_mid ;
        double p_now;
    
        double kg;
    
        x_mid=x_last;                       //x_last=x(k-1|k-1),x_mid=x(k|k-1)
        p_mid=p_last+Q;                     //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
    
        /*
         *  卡尔曼滤波的五个重要公式
         */
        kg=p_mid/(p_mid+R);                 //kg为kalman filter,R 为噪声
        x_now=x_mid+kg*(ResrcData-x_mid);   //估计出的最优值
        p_now=(1-kg)*p_mid;                 //最优值对应的covariance
        p_last = p_now;                     //更新covariance 值
        x_last = x_now;                     //更新系统状态值
    
        return x_now;
    
    }
    

    我们将经过滤波的数据和没有滤波的通过单片机的串口发送出来将数据导入到matlab中,可以看到效果如下
    在这里插入图片描述
    可以看到实际的工程应用效果也是比较明显的,符合我们的预期。

    展开全文
  • 卡尔曼滤波的特点就是采用递归方法解决线性滤波问题,只需要知道当前的测量值和上一时刻的最优值,就能对此刻进行最优值计算,计算量小,不需要大量储存空间,适合性能不太强的单片机处理。二阶卡尔曼滤波更加可靠,...

    卡尔曼滤波

      滤波的方法有很多种,针对不同的情况选用的最优滤波方法也是不同的。卡尔曼滤波的特点就是采用递归方法解决线性滤波问题,只需要知道当前的测量值和上一时刻的最优值,就能对此刻进行最优值计算,计算量小,不需要大量储存空间,适合性能不太强的单片机处理。二阶卡尔曼滤波更加可靠,但计算量较大,通常使用的是一阶。

    现在网络上卡尔曼滤波的资料有很多,大多是一位大佬生产,说不清的码农搬砖,想要真正理解卡尔曼滤波的道理,还需静下心来从数学理论慢慢体会。分享一个正在研究的文档,比较深入的那种。

    https://pan.baidu.com/s/11NCpqgciVc1KIx4H66upAA

    既然要真正搞懂很难,那我就反其道而行之,边用边学吧。

    卡尔曼滤波由五个基本方程式组成,列出了方程式,滤波也就完成了。

    深入一点的方程式从那个文章中拷进来的,看一下就好,我们分析浅显点的。

    先把初始化赋值说一下,免得直接懵逼。在飞控中初始化时常用赋值如下:P(k-1)=0.02     Kg(k)=P(k)=X(k)=0    Q=0.001    R=0.5  

    1.预测状态方程

    X(k)=A X(k-1)+B U(k)      -----深

    X(k)= X(k-1)            ----------浅

    简单的说就是把上一时刻的卡尔曼滤波的最优值 X(k-1)  乘以一个系数赋给当前预测值,这个系数常为1。U(k) 为控制增益,一般为0。

    2.预测协方差方程

    P(k)=A P(k-1) A’+Q   -----深

    P(k)= P(k-1) +Q     ----------浅

    P(k)是此刻系统协方差,P(k-1)是k-1时刻系统协方差,Q是系统过程噪声的协方差。在飞控中,这些值的初始化值是根据经验和计算得来的,最后统一赋一下。

    3.卡尔曼增益方程

    Kg(k)= P(k) H’ / (H P(k) H’ + R)         -------深

    Kg(k)=P(k)/(P(k)+R)     ----------浅

    Kg(k)叫做卡尔曼增益,P(k)是刚算出来的此刻系统协方差,R是对象测量噪声的协方差,初始化时直接给出,调试时可以根据需要调整大小,R大点时波形幅度减小,效果滞后,小点时相反,根据效果调整就好。

    4.更新最优值方程

    X(k)= X(k)+Kg(k) (Z(k)-H X(k))      -------深

    X(k)= X(k)+Kg(k) (Z(k)- X(k))     ----------浅

    X(k)就此刻的最优值,可以直接用的那种,X(k)是上一时刻的最优值,Z(k)是此刻对象的测量值,换句话说就是卡尔曼滤波函数的输入值,要是测量值都没了,还滤毛线啊,是吧。

    5.更新协防差方程

    P(k-1)= (I-Kg(k) H )P(k)      -------深

    P(k-1)= (I-Kg(k) )P(k)      ----------浅

    此刻的系统协方差一经计算变成旧的协方差,供下次使用。

    这就结束了,被滤波的值是Z(k),滤波的结果是X(k)

     

    转载于:https://www.cnblogs.com/Traveler-Wind/p/10359594.html

    展开全文
  • 实战低通滤波和卡尔曼滤波

    热门讨论 2020-05-09 18:12:47
    滤波的方法有太多了,从简单的均值滤波(其实我认为应该叫状态估计)、中值滤波到能把飞船送上月球的卡尔曼滤波。往往我们都会选择适合工程需求的滤波器对信号进行滤波。那么本次就从理论到实践,从公式推导到算法...
  • 卡尔曼滤波 -- 从推导到应用(一)

    千次阅读 2017-06-19 16:39:19
    第一部分,结合例子,从最小均方差的角度,直观地介绍卡尔曼滤波的原理,并给出较为详细的数学推导。 第二部分,通过两个例子给出卡尔曼滤波的实际应用。其中将详细介绍一个匀加速模型,并直观
  • 控制领域,获取控制对象**精确**的...但是在传感器测量过程中由于**自身误差**和**外部干扰**导致采样值不准确,卡尔曼滤波的作用就是为了**纠正**(correct)这些不准确,从而得到较准确的“**状态值**”(采样数据);
  • 卡尔曼滤波在单片机上的使用

    千次阅读 2017-03-06 17:30:02
    卡尔曼滤波在单片机上的使用原文出处#ifndef _KALMAN_H_ #define _KALMAN_H_extern KalmanGain;// 卡尔曼增益 extern EstimateCovariance;//估计协方差 extern MeasureCovariance;//测量协方差 extern EstimateValue...
  • github:...============================================================ 其他相关卡尔曼滤波博客: 单片机开发 | 基于51单片机实现MPU6050的卡尔曼滤波算法(代码类) 单片机开发 | 基...
  • #include "sys.h" #include "delay.h" #include "usart.h" #include "led.h" #include "timer.h" #include "beep.h" #include "bmp.h" #include "oled.h" u8 table1[]={0X03,0XFC};//µ÷²¨ÉèÖà ...
  • 一直以来用过很多次KALMAN滤波,没有进行系统的总结,本次笔记将对此进行从原理到推导到代码实现及应用进行总结和分析,阐述一些个人使用的观点。首先,了解该滤波的原理及使用场景和应用范畴,然后,说明使用此滤波...
  • 一阶卡尔曼学习记录

    千次阅读 2018-04-22 11:30:53
    卡尔曼滤波是时域估计方法,能对时变、非平稳信号、多维信号进行处理,不需要频域变换,采用递推算法,运算量小,存贮量小,实现简单。2.卡尔曼滤波器原理卡尔曼有五个基本公式,前两个是预测公式,后三个是更新公.....
  • 资源下载: 源代码: http://download.csdn.net/download/feng3121/10262828 功能图与程序框图: http://download.csdn.net/download/feng3121/10262796更新MPU6050细节:GY521MPU6050是I2C元件,需要用I2C时序...
  • 卡尔曼滤波算法图概述 想象一下,你在一辆装有外部传感器的汽车里。汽车传感器可以检测到四处移动的物体:例如,传感器可能检测到自行车。 卡尔曼滤波器算法将执行以下步骤: 第一次测量 - 过滤器将接收...
  • 用于c51单片机编程,用于消除adc转化时的抖动。
  • 一、arduino版 限幅滤波法(又称程序判断滤波法) 中位值滤波法 ... 新增加 卡尔曼滤波(非扩展卡尔曼) 程序默认对int类型数据进行滤波,如需要对其他类型进行滤波,只需要把程序中所有int替换成l...
  • 5种常用的四轴飞行器PID算法讲解集合

    万次阅读 多人点赞 2016-04-05 09:04:38
    在某莫上看到的,...三角函数直接解算欧拉角+卡尔曼滤波+单级PID版本 效果:卡尔曼滤波噪声偏大,滞后略微严重,单级PID难操作,打舵响应慢,跟随效应差。不过比较适合初学四轴的人,难度比四元数加串级PID版本
  • 关于MPU6050一阶互补滤波方法(从原理到代码实现) 1.写在前面 最近知道自己不用考研后便花了很多时间来准备机械创新设计大赛,在设计的多功能防台风窗中需要到MPU6050对窗户的姿态进行检测,用来反馈到步进电机控制...
1 2 3 4 5 ... 10
收藏数 185
精华内容 74
关键字:

单片机控制卡尔曼滤波