精华内容
下载资源
问答
  • 分享个基于Labview写的卡尔曼滤波程序
  • 简单的卡尔曼滤波算法,适合初学者的学习和应用,同时可以在此基础上,进行改进。
  • 原始卡尔曼滤波算法(KF)、扩展卡尔曼滤波算法(EKF)以及无迹卡尔曼滤波算法(UKF)三者之间的区别? 原文:https://www.zhihu.com/question/22714163/answer/87629897 KF针对于线性高斯的情况,EKF针对于非...

    原始卡尔曼滤波算法(KF)、扩展卡尔曼滤波算法(EKF)以及无迹卡尔曼滤波算法(UKF)三者之间的区别?

    原文:https://www.zhihu.com/question/22714163/answer/87629897

     

    KF针对于线性高斯的情况,EKF针对于非线性高斯,其是将非线性部分进行一阶泰勒展开,因此忽 略了高阶项,误差较大,UKF是将UT变换与KF结合的产物,它的基础理念是接近一个非线性函数的 概率分布非接近其本身更简单。后两种卡尔曼是针对同一问题的不同思路的解决方案,其实UKF的 能力已经跳出了非线性高斯的范围,其也可以解决非高斯问题,只不过在这方面PF能做的更好,运 算量也更大。

     

    1.KF是线性滤波器,不多解释。2.说到非线性卡尔曼滤波器,是由贝叶斯滤波理论得出统一的理论框架:非线性卡尔曼滤波器的状态估计可等效为多维向量积分的计算。积分形式可表示为非线性函数x高斯概率密度函数。但该积分的闭式解很难求解,所以得对高斯加权的多维非线性函数的积分进行近似。近似主要分两大类:非线性函数的近似和高斯概率密度函数的近似。

    非线性函数的近似有基于泰勒级数展开的ekf,基于差分的DDF和多项式卡尔曼(比如 雪夫多项式等)。

    概率密度函数的近似主要有无迹变换(ukf),容积卡尔曼,中心差分卡尔曼,或是把协方差矩阵分解构成基于平方根的卡尔曼滤波器等。所以卡尔曼滤波器都是对后验密度做假设的次优估计。而粒子滤波,高斯混合滤波器,自适应网格点群滤波器等是不对后验概率密度做任何假设的最优估计。不过计算量挺大的。像基于ukf+srcdkf的位置状态估计,在进行时间更新的时候,就需要更新1700多个数据。要是采用粒子滤波计算量就更大……


    图说卡尔曼滤波,一份通俗易懂的教程

    原文:https://zhuanlan.zhihu.com/p/39912633

    作者:Bzarg

    编译:Bot

    编者按:卡尔曼滤波(Kalman filter)是一种高效的自回归滤波器,它能在存在诸多不确定性情况的组合信息中估计动态系统的状态,是一种强大的、通用性极强的工具。它的提出者,鲁道夫.E.卡尔曼,在一次访问NASA埃姆斯研究中心时,发现这种方法能帮助解决阿波罗计划的轨道预测问题,后来NASA在阿波罗飞船的导航系统中确实也用到了这个滤波器。最终,飞船正确驶向月球,完成了人类历史上的第一次登月。

    本文是国外博主Bzarg在2015年写的一篇图解。虽然是几年前的文章,但是动态定位、自动导航、时间序列模型、卫星导航——卡尔曼滤波的应用范围依然非常广。那么,作为软件工程师和机器学习工程师,你真的了解卡尔曼滤波吗?

    什么是卡尔曼滤波?

    对于这个滤波器,我们几乎可以下这么一个定论:只要是存在不确定信息的动态系统,卡尔曼滤波就可以对系统下一步要做什么做出有根据的推测。即便有噪声信息干扰,卡尔曼滤波通常也能很好的弄清楚究竟发生了什么,找出现象间不易察觉的相关性。

    因此卡尔曼滤波非常适合不断变化的系统,它的优点还有内存占用较小(只需保留前一个状态)、速度快,是实时问题和嵌入式系统的理想选择。

    如果你曾经Google过卡尔曼滤波的教程(如今有一点点改善),你会发现相关的算法教程非常可怕,而且也没具体说清楚是什么。事实上,卡尔曼滤波很简单,如果我们以正确的方式看它,理解是很水到渠成的事。

    本文会用大量清晰、美观的图片和颜色来解释这个概念,读者只需具备概率论和矩阵的一般基础知识。

    我们能用卡尔曼滤波做什么?

    让我们举个例子:你造了一个可以在树林里四处溜达的小机器人,为了让它实现导航,机器人需要知道自己所处的位置。

    也就是说,机器人有一个包含位置信息和速度信息的状态 [公式] :

    注意,在这个例子中,状态是位置和速度,放进其他问题里,它也可以是水箱里的液体体积、汽车引擎温度、触摸板上指尖的位置,或者其他任何数据。

    我们的小机器人装有GPS传感器,定位精度10米。虽然一般来说这点精度够用了,但我们希望它的定位误差能再小点,毕竟树林里到处都是土坑和陡坡,如果机器人稍稍偏了那么几米,它就有可能滚落山坡。所以GPS提供的信息还不够充分。

    我们也可以预测机器人是怎么移动的:它会把指令发送给控制轮子的马达,如果这一刻它始终朝一个方向前进,没有遇到任何障碍物,那么下一刻它可能会继续坚持这个路线。但是机器人对自己的状态不是全知的:它可能会逆风行驶,轮子打滑,滚落颠簸地形……所以车轮转动次数并不能完全代表实际行驶距离,基于这个距离的预测也不完美。

    这个问题下,GPS为我们提供了一些关于状态的信息,但那是间接的、不准确的;我们的预测提供了关于机器人轨迹的信息,但那也是间接的、不准确的。

    但以上就是我们能够获得的全部信息,在它们的基础上,我们是否能给出一个完整预测,让它的准确度比机器人搜集的单次预测汇总更高?用了卡尔曼滤波,这个问题可以迎刃而解。

    卡尔曼滤波眼里的机器人问题

    还是上面这个问题,我们有一个状态,它和速度、位置有关:

    我们不知道它们的实际值是多少,但掌握着一些速度和位置的可能组合,其中某些组合的可能性更高:

    卡尔曼滤波假设两个变量(在我们的例子里是位置和速度)都应该是随机的,而且符合高斯分布。每个变量都有一个均值 [公式] ,它是随机分布的中心;有一个方差 [公式] ,它衡量组合的不确定性。

    在上图中,位置和速度是不相关的,这意味着我们不能从一个变量推测另一个变量。

    那么如果位置和速度相关呢?如下图所示,机器人前往特定位置的可能性取决于它拥有的速度。

    这不难理解,如果基于旧位置估计新位置,我们会产生这两个结论:如果速度很快,机器人可能移动得更远,所以得到的位置会更远;如果速度很慢,机器人就走不了那么远。

    这种关系对目标跟踪来说非常重要,因为它提供了更多信息:一个可以衡量可能性的标准。这就是卡尔曼滤波的目标:从不确定信息中挤出尽可能多的信息!

    为了捕获这种相关性,我们用的是协方差矩阵。简而言之,矩阵的每个值是第 [公式] 个变量和第 [公式] 个变量之间的相关程度(由于矩阵是对称的, [公式] 和 [公式] 的位置可以随便交换)。我们用 [公式] 表示协方差矩阵,在这个例子中,就是 [公式] 。

    用矩阵描述问题

    为了把以上关于状态的信息建模为高斯分布(图中色块),我们还需要 [公式] 时的两个信息:最佳估计 [公式] (均值,也就是 [公式] ),协方差矩阵 [公式] 。(虽然还是用了位置和速度两个变量,但只要和问题相关,卡尔曼滤波可以包含任意数量的变量)

    接下来,我们要通过查看当前状态(k-1时)来预测下一个状态(k时)。这里我们查看的状态不是真值,但预测函数无视真假,可以给出新分布:

    我们可以用矩阵 [公式] 表示这个预测步骤:

    它从原始预测中取每一点,并将其移动到新的预测位置。如果原始预测是正确的,系统就会移动到新位置。

    这是怎么做到的?为什么我们可以用矩阵来预测机器人下一刻的位置和速度?下面是个简单公式:

    换成矩阵形式:

    这是一个预测矩阵,它能给出机器人的下一个状态,但目前我们还不知道协方差矩阵的更新方法。这也是我们要引出下面这个等式的原因:如果我们将分布中的每个点乘以矩阵A,那么它的协方差矩阵会发生什么变化

    把这个式子和上面的最佳估计 [公式] 结合,可得:

    外部影响

    但是,除了速度和位置,外因也会对系统造成影响。比如模拟火车运动,除了列车自驾系统,列车操作员可能会手动调速。在我们的机器人示例中,导航软件也可以发出停止指令。对于这些信息,我们把它作为一个向量 [公式] ,纳入预测系统作为修正。

    假设油门设置和控制命令是已知的,我们知道火车的预期加速度 [公式] 。根据运动学基本定理,我们可得:

    把它转成矩阵形式:

    [公式] 是控制矩阵, [公式] 是控制向量。如果外部环境异常简单,我们可以忽略这部分内容,但是如果添加了外部影响后,模型的准确率还是上不去,这又是为什么呢?

    外部不确定性

    当一个国家只按照自己的步子发展时,它会自生自灭。当一个国家开始依赖外部力量发展时,只要这些外部力量是已知的,我们也能预测它的存亡。

    但是,如果存在我们不知道的力量呢?当我们监控无人机时,它可能会受到风的影响;当我们跟踪轮式机器人时,它的轮胎可能会打滑,或者粗糙地面会降低它的移速。这些因素是难以掌握的,如果出现其中的任意一种情况,预测结果就难以保障。

    这要求我们在每个预测步骤后再加上一些新的不确定性,来模拟和“世界”相关的所有不确定性:

    如上图所示,加上外部不确定性后, [公式] 的每个预测状态都可能会移动到另一点,也就是蓝色的高斯分布会移动到紫色高斯分布的位置,并且具有协方差 [公式] 。换句话说,我们把这些不确定影响视为协方差 [公式] 的噪声。

    这个紫色的高斯分布拥有和原分布相同的均值,但协方差不同。

    我们在原式上加入 [公式] :

    简而言之,这里:

    [公式] 是基于 [公式] 和 [公式] 校正后得到的预测。

    [公式] 是基于 [公式] 和 [公式] 得到的预测。

    现在,有了这些概念介绍,我们可以把传感器数据输入其中。

    通过测量来细化估计值

    我们可能有好几个传感器,它们一起提供有关系统状态的信息。传感器的作用不是我们关心的重点,它可以读取位置,可以读取速度,重点是,它能告诉我们关于状态的间接信息——它是状态下产生的一组读数。

    请注意,读数的规模和状态的规模不一定相同,所以我们把传感器读数矩阵设为 [公式] 。

    把这些分布转换为一般形式:

    卡尔曼滤波的一大优点是擅长处理传感器噪声。换句话说,由于种种因素,传感器记录的信息其实是不准的,一个状态事实上可以产生多种读数。

    我们将这种不确定性(即传感器噪声)的协方差设为 [公式] ,读数的分布均值设为 [公式] 。

    现在我们得到了两块高斯分布,一块围绕预测的均值,另一块围绕传感器读数。

    如果要生成靠谱预测,模型必须调和这两个信息。也就是说,对于任何可能的读数 [公式] ,这两种方法预测的状态都有可能是准的,也都有可能是不准的。重点是我们怎么找到这两个准确率。

    最简单的方法是两者相乘:

    两块高斯分布相乘后,我们可以得到它们的重叠部分,这也是会出现最佳估计的区域。换个角度看,它看起来也符合高斯分布:

    事实证明,当你把两个高斯分布和它们各自的均值和协方差矩阵相乘时,你会得到一个拥有独立均值和协方差矩阵的新高斯分布。最后剩下的问题就不难解决了:我们必须有一个公式来从旧的参数中获取这些新参数!

    结合高斯

    让我们从一维看起,设方差为 [公式] ,均值为 [公式] ,一个标准一维高斯钟形曲线方程如下所示:

    那么两条高斯曲线相乘呢?

    把这个式子按照一维方程进行扩展,可得:

    如果有些太复杂,我们用k简化一下:

     

    以上是一维的内容,如果是多维空间,把这个式子转成矩阵格式:

    这个矩阵 [公式] 就是我们说的卡尔曼增益,easy!

    把它们结合在一起

    截至目前,我们有用矩阵 [公式] 预测的分布,有用传感器读数 [公式] 预测的分布。把它们代入上节的矩阵等式中:

    相应的,卡尔曼增益就是:

    考虑到 [公式] 里还包含着一个 [公式] ,我们再精简一下上式:

    最后, [公式] 是我们的最佳估计值,我们可以把它继续放进去做另一轮预测:

    希望这篇文章能对你有用!

    展开全文
  • 本文以rssi(接收信号强度)滤波为背景,结合卡尔曼的五个公式,设计 rssi 一维卡尔曼滤波器,用MATLAB语言实现一维卡尔曼滤波器,并附上代码和滤波结果图; 本文工分为以下几个部分: 1、引言 2、模型的系统方程和...

    一、引言

    本文以rssi(接收信号强度)滤波为背景,结合卡尔曼的五个公式,设计 rssi 一维卡尔曼滤波器,用MATLAB语言实现一维卡尔曼滤波器,并附上代码和滤波结果图;

    本文工分为以下几个部分:

    1、引言

    2、模型的系统方程和状态方程

    3、卡尔曼滤波过程及五个基本公式

    4、公式中每个参数详细注释

    5、结合rssi滤波实例设计滤波器

    6、MATLAB实现滤波器

     

    二、模型的系统方程和状态方程

    • 系统的状态方程:

    \LARGE {\color{DarkBlue} }x_k = Ax_{k-1} + Bu_{k-1} + w_{k-1}

    状态方程是根据上一时刻的状态和控制变量来推测当前时刻的状态,\large w_{k-1}是服从高斯分布的噪声,是预测过程的噪声,它对应了 \large x_k 中每个分量的噪声,是期望为 0,协方差为 Q 的高斯白噪声\large w_{k-1} \sim N_{(0, Q))},Q即下文的过程激励噪声Q.

    • 系统的观测方程:

    \LARGE z_k = Hx_k + v_k

    观测方式是当前时刻的量测信息,\large v_k是观测的噪声,服从高斯分布,\large v_k \sim N_{(0,R)},R即下文的测量噪声R。

    • 卡尔曼滤波算法有两个基本假设:

    ( 1) 信息过程的足够精确的模型,是由白噪声所激发的线性( 也可以是时变的) 动态系统;

    ( 2) 每次的测量信号都包含着附加的白噪声分量 。当满足以上假设时,可以应用卡尔曼滤波算法。

     

    三、 卡尔曼滤波过程及五个基本公式

    • 卡尔曼滤波时间更新(预测)
    • 1. 向前推算状态变量  

    \LARGE \hat{x}_{k}^{-} = A\hat{x}_{k-1} + Bu_{k-1}

    • 2. 向前推算误差协方差

    \LARGE P_{k}^{-} = AP_{k-1} A^{T} + Q

    • 卡尔曼滤波测量更新(校正)

    • 3. 计算卡尔曼增益

    \LARGE K_k = \frac{P_{k}^{-}{H^T}}{HP_{k}^{-}H^T + R}

    • 4. 由观测变量\large z_k更新估计

    \LARGE \hat{x}_k = \hat{x}_{k}^{-} + K_k(z_k - H\hat{x}_{k}^{-})

    • 5. 更新测量误差

    \LARGE P_k = (I - K_kH)P_{k}^{-}

     

    四、 公式中每个参数详细注释

    1. \LARGE \hat{x}_{k-1}\LARGE \hat{x}_{k}:   分别表示 \LARGE k-1时刻和 \LARGE k时刻的后验状态估计值,是滤波的结果之一,即更新后的结果,也叫最优估计(估计的状态,根据理论,我们不可能知道每时刻状态的确切结果所以叫估计)。

       

    2. \LARGE \hat{x}_{k}^{-}:   \LARGE k 时刻的先验状态估计值,是滤波的中间计算结果,即根据上一时刻(\LARGE k-1时刻)的最优估计预测的\LARGE k时刻的结果,是预测方程的结果。

       

    3. \LARGE P_{k-1} 和 \LARGE P_k:  分别表示 k - 1 时刻和 k 时刻的后验估计协方差(即\LARGE \hat{x}_{k-1} 和 \LARGE \hat{x}_k 的协方差,表示状态的不确定度),是滤波的结果之一。

       

    4. \LARGE P_{k}^{-}:  k 时刻的先验估计协方差(\LARGE \hat{x}_{k}^{-}的协方差),是滤波的中间计算结果。

       

    5. \LARGE H: 是状态变量到测量(观测)的转换矩阵,表示将状态和观测连接起来的关系,卡尔曼滤波里为线性关系,它负责将 m 维的测量值转换到 n 维,使之符合状态变量的数学形式,是滤波的前提条件之一。

       

    6. \LARGE z_k: 测量值(观测值),是滤波的输入。

       

    7. \LARGE K_k: 滤波增益矩阵,是滤波的中间计算结果,卡尔曼增益,或卡尔曼系数。

       

    8. \LARGE A: 状态转移矩阵,实际上是对目标状态转换的一种猜想模型。例如在机动目标跟踪中, 状态转移矩阵常常用来对目标的运动建模,其模型可能为匀速直线运动或者匀加速运动。当状态转移矩阵不符合目标的状态转换模型时,滤波会很快发散。

       

    9. \LARGE Q : 过程激励噪声协方差(系统过程的协方差)。该参数被用来表示状态转换矩阵与实际过程之间的误差。因为我们无法直接观测到过程信号, 所以 Q 的取值是很难确定的。是卡尔曼滤波器用于估计离散时间过程的状态变量,也叫预测模型本身带来的噪声。状态转移协方差矩阵。

       

    10. \LARGE R: 测量噪声协方差。滤波器实际实现时,测量噪声协方差 R一般可以观测得到,是滤波器的已知条件。

       

    11. \LARGE B: 是将输入转换为状态的矩阵。

       

    12. \LARGE (z_k - H\hat{x}_{k}^{-}): 实际观测和预测观测的残差,和卡尔曼增益一起修正先验(预测),得到后验。

     

    五、 结合rssi滤波实例设计滤波器

    • 1、 建立模型系统方程和量测方程

    由于分析对象是无线信号的一维rssi状态,所以具体空间过程不关心,只需要从发射端发射到接收端接收是没有其他控制状态的,但是在传输过程中是存在噪声的,根据公式

    \LARGE {\color{DarkBlue} }x_k = Ax_{k-1} + Bu_{k-1} + w_{k-1}

    可得:A 为[1],B为[0],\large w_{k-1}为高斯白噪声可不关心

    • 2、建立量测方程

    由于接收设备可直接输出rssi值,根据公式

    \LARGE z_k = Hx_k + v_k

    可得:H为[1],\large v_k为量测噪声可不关心

    • 3、分析Q和R

    假如\large w已经分析出一系列的数据,则\large Q = cov(w);

    假如\large v已经分析出一些列的数据,则\large R = cov(v);

    • 4、初始值确定

    给滤波过程的初始状态初始化。

    • 5、最后一步,就是对照公式根据理解,套公式,写程序。

     

    六、MATLAB实现滤波器 

    • 1、 滤波器设计
    function z = kalmanFilter(x)
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %
    %  function z = kalmanFilter(x)
    %
    %>
    %> @brief 一维卡尔曼滤波
    %>
    %> @param[out]  z             滤波后的结果
    %> @param[in]   x             需要滤波的数据
    %>
    %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
        % 卡尔曼相关变量定义
        persistent xk xk_1;             % 状态量
        persistent zk;                  % 观测量
        persistent A;                   % 状态转移矩阵
    %     persistent B;                   % 控制输入模型
        persistent H;                   % 观测矩阵
        persistent Pk Pk_1;             % 误差协方差矩阵
        persistent Q;                   % 状态噪声协方差矩阵
        persistent R;                   % 观测噪声协方差矩阵
        
        % 卡尔曼相关参数初始化
        if isempty(xk)
            A = 1;
            H = 1;
            Pk = 1;
            Pk_1 = 1;                   % 初始误差协方差为1
            Q = 0.01;                   % 反应两个时刻rssi方差
            R = 0.05;                   % 反应测量rssi的测量精度
            xk = 0;
            xk_1 = 0;
            zk = 0;
        end
        
        I = 1;
        if xk_1 == 0
            xk_1 = x;
            xk = x;
        else
            zk = H*x;                   % 观测量方程
            % 预测
            X = A*xk_1;                 % 状态预测
            P = A*Pk_1*A' + Q;          % 误差协方差预测
            % 更新(校正)
            K = P*H'*inv(H*P*H'+R);     % 卡尔曼增益更新
            xk = X + K*(zk - H*X);      % 更新校正
            xk_1 = xk;                  % 保存校正后的值,下一次滤波使用
            Pk = (I - K*H)*P;           % 更新误差协方差
            Pk_1 = Pk;                  % 保存校正后的误差协方差,下一次滤波使用
        end
        
        % 滤波结果返回
        z = xk;
    end
    • 2、仿真运行
    %% 1. 导入数据
    [fname, pname] = uigetfile('*', 'Sample Dialog Box');
    fileID = fopen(strcat(pname, fname));
    data = cell2mat(textscan(fileID,'%f%f','delimiter', ',','headerlines',0));
    rssi = data(:, 1);
    
    %% 2. 卡尔曼滤波
    rssi_opt = zeros(size(rssi,1), 1);
    for k = 1:size(rssi,1)
        rssi_opt(k) = kalmanFilter(rssi(k));
    end
    
    %% 3. 滤波检验
    figure(1);
    plot(rssi, 'Color', 'r', 'Marker', 'o'); hold on;
    plot(rssi_opt, 'Color', 'b', 'LineStyle', '-', 'Marker', '+'); hold off;
    legend('rssi原始波形', 'rssi经过滤波后的波形');
    title('rssi滤波对比');
    • 3、运行结果

    展开全文
  • 01-16 20:39:20 11340 收藏 111分类专栏: C语言嵌入式 文章标签: 卡尔曼滤波 C代码卡尔曼滤波理论很容易就可以在MATLAB软件环境下实现,但是,实际的硬件板子上还是需要C语言,当然可以自动代码生成,还有种就是...

    a往南向北 2019-01-16 20:39:20  11340  收藏 111

    分类专栏: C语言嵌入式 文章标签: 卡尔曼滤波 C代码

    卡尔曼滤波理论很容易就可以在MATLAB软件环境下实现,但是,实际的硬件板子上还是需要C语言,当然可以自动代码生成,还有一种就是直接手动编写C语言。

    1.前言

    在google上搜索卡尔曼滤波,很容易找到以下这个帖子:http://blog.csdn.net/lanbing510/article/details/8828109

    帖子最后用matlab实现了kalman,然后博主的前面一些帖子也有详细转载相关贴子,自己也给出了一些源代码,例如转载的这篇卡尔曼滤波器通俗介绍:https://blog.csdn.net/weixin_38451800/article/details/85462129

    2.卡尔曼滤波的C语言

    网上很多的是关于多维kalman实现。参照网上的一些代码,本文实现了一个一维地滤波,对于有C语言基础的同学来讲,理解起来应该很容易了。

    百度百科里面有这个帖子:http://wenku.baidu.com/view/8523cb6eaf1ffc4ffe47ac24.html

    讲解的是一维kalman滤波器,但是最后printf出来的都是分立的值,看不出来什么。参考那段代码,改写成了下面这段代码,在labwindows里面绘制了一段曲线,效果就很直观了:

    /*-------------------------------------------------------------------------------------------------------------*/

    void KalmanFilter(unsigned int ResrcDataCnt,const double *ResrcData,double *FilterOutput,double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction)

    {

    unsigned int i;

    double R = MeasureNoise_R;

    double Q = ProcessNiose_Q;

    double x_last = *ResrcData;

    double x_mid = x_last;

    double x_now;

    double p_last = InitialPrediction;

    double p_mid ;

    double p_now;

    double kg;

    for(i=0;i

    {

    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+i)-x_mid);//估计出的最优值

    p_now=(1-kg)*p_mid;//最优值对应的covariance

    *(FilterOutput + i)  = x_now;

    p_last = p_now; //更新covariance值

    x_last = x_now; //更新系统状态值

    }

    }

    /*-------------------------------------------------------------------------------------------------------------*/

    参考上面的代码,优化了一下之后(运行在STM32上):

    /*-------------------------------------------------------------------------------------------------------------*/

    /*

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

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

    */

    double KalmanFilter(const double ResrcData,

    double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction)

    {

    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;

    }

    /*-------------------------------------------------------------------------------------------------------------*/

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    3.接下来,是另外一个版本的单维卡尔曼滤波的C语言源代码:

    #include "stdio.h"

    #include "stdlib.h"

    #include "math.h"

    double frand()

    { return 2*((rand()/(double)RAND_MAX) - 0.5); // 随机噪声}

    void main()

    { float x_last=0;

    float p_last=0.02;

    float Q=0.018;

    float R=0.542;

    float kg;

    float x_mid;

    float x_now;

    float p_mid;

    float p_now;

    float z_real=0.56;//0.56

    float z_measure;

    float sumerror_kalman=0;

    float sumerror_measure=0;

    int i;

    x_last=z_real+frand()*0.03;

    x_mid=x_last;

    for(i=0;i<20;i++)

    { 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为噪声

    z_measure=z_real+frand()*0.03;                      // 测量值

    x_now=x_mid+kg*(z_measure-x_mid);                   // 估计出的最优值

    p_now=(1-kg)*p_mid;                                 // 最优值对应的covariance

    printf("Real position: %6.3f \n",z_real);           // 显示真值

    printf("Mesaured position: %6.3f [diff:%.3f]\n",z_measure,fabs(z_real-z_measure));

    // 显示测量值以及真值与测量值之间的误差

    printf("Kalman position: %6.3f [diff:%.3f]\n",x_now,fabs(z_real - x_now));  // 显示kalman 估计值以及真值和卡尔曼估计值的误差

    sumerror_kalman += fabs(z_real - x_now);            //kalman 估计值的累积误差

    sumerror_measure += fabs(z_real-z_measure);         // 真值与测量值的累积误差

    p_last = p_now;                                     // 更新covariance 值

    x_last = x_now;                                     // 更新系统状态值

    }

    printf(" 总体测量误差 : %f\n",sumerror_measure);     // 输出测量累积误差

    printf(" 总体卡尔曼滤波误差: %f\n",sumerror_kalman);  // 输出kalman 累积误差

    printf(" 卡尔曼误差所占比例: %d%% \n",100-(int)((sumerror_kalman/sumerror_measure)*100));

    ——————————————————————————————————————————

    部分内容转载自:https://www.amobbs.com/thread-5571611-1-1.html 和https://wenku.baidu.com/view/8523cb6eaf1ffc4ffe47ac24.html

    展开全文
  • 深入理解卡尔曼滤波算法

    千次阅读 2020-10-27 21:10:56
    卡尔曼滤波算法从名称上来看落脚点是个滤波算法,一般的滤波算法都是频域滤波,而卡尔曼滤波算法个时域滤波,这就是它的强大之处。再说说滤波这个名词,本质上就是给加权,其实深度学习中的卷积操作也是加权...

    最近做卡尔曼滤波跟踪的项目,看原理花了一天,再网上查找并看懂别人的kalman c++代码花了我近三天的时间。卡尔曼滤波就是纸老虎,核心原理不难,核心公式就5个,2个状态预测更新公式,3个矫正公式。这里只讲解线性kalman滤波模型,非线性kalman滤波可以用扩散kalman滤波算法。

    概述

    卡尔曼滤波算法从名称上来看落脚点是一个滤波算法,一般的滤波算法都是频域滤波,而卡尔曼滤波算法是一个时域滤波,时域就是它的强大之处。卡尔曼滤波也是一种最优估计算法,常见的最优估计算法有“最小二乘法”等,卡尔曼滤波也是一个迭代器,根据以知的先验值,预测下一时刻的估计值。
    再说说滤波这个名词,本质上就是给加权。既然是加权,那么卡尔曼滤波算法本质就是数据融合的操作(Data fusion),卡尔曼增益就是融合过程中的权重,融合的内容有两个:分别是满足一定分布的先验状态估计值和满足一定分布的观测值之间的融合(也就是你们可能在其他资料上看到的两个椭圆相乘,其实就是数据的融合),融合后得到后验状态估计值。

    其实深度学习中的卷积操作也是加权求和,本质上也是滤波,本质上也是求导,比如边缘检测的离散微分算子sobel的计算过程,也是分别在x和y两个方向上求导,提取出边缘特征。(下面是我个人的思考)由点及面,我们就可以想像到一个大尺寸的卷积核,其实就相当于“变形求导”,通过卷积核内部权重数值大小给定求导曲线,卷积就是求解在这条曲线上的导数。 一个卷积核是提取特征,那么多个卷积核组合起来就有无限可能,提取到的就是高维特征(高维特征对应图片的纹理等深层次的语义信息),组合起来非常之强大,就能够实现深度学习中的分类、边框回归等功能。

    直接上菜 - 牛逼哄哄的五大公式

    在这里插入图片描述
    下面来一个个详细剖析每个参数:
    1.在这里插入图片描述分别表示 k - 1 时刻和 k 时刻的后验状态估计值,是滤波的结果之一,即更新后的结果,也叫最优估计(估计的状态,根据理论,我们不可能知道每时刻状态的确切结果所以叫估计)。
    2,在这里插入图片描述: k 时刻的先验状态估计值,是滤波的中间计算结果,即根据上一时刻(k-1时刻)的最优估计预测的k时刻的结果,是预测方程的结果。
    3,在这里插入图片描述:分别表示 k - 1 时刻和 k 时刻的后验估计协方差(即的协方差,表示状态的不确定度),是滤波的结果之一。

    4,在这里插入图片描述:k 时刻的先验估计协方差(的协方差),是滤波的中间计算结果。

    5,H:是状态变量到测量(观测)的转换矩阵,表示将状态和观测连接起来的关系,卡尔曼滤波里为线性关系,它负责将 m 维的测量值转换到 n 维,使之符合状态变量的数学形式,是滤波的前提条件之一。

    6,在这里插入图片描述:测量值(观测值),是滤波的输入。

    7,在这里插入图片描述:滤波增益矩阵,是滤波的中间计算结果,卡尔曼增益,或卡尔曼系数。

    8,A:状态转移矩阵,实际上是对目标状态转换的一种猜想模型。例如在机动目标跟踪中, 状态转移矩阵常常用来对目标的运动建模,其模型可能为匀速直线运动或者匀加速运动。当状态转移矩阵不符合目标的状态转换模型时,滤波会很快发散。

    9,Q:过程激励噪声协方差(系统过程的协方差)。该参数被用来表示状态转换矩阵与实际过程之间的误差。因为我们无法直接观测到过程信号, 所以 Q 的取值是很难确定的。是卡尔曼滤波器用于估计离散时间过程的状态变量,也叫预测模型本身带来的噪声。状态转移协方差矩阵

    10:R: 测量噪声协方差。滤波器实际实现时,测量噪声协方差 R一般可以观测得到,是滤波器的已知条件。

    11,B:是将输入转换为状态的矩阵

    12,在这里插入图片描述:实际观测和预测观测的残差,和卡尔曼增益一起修正先验(预测),得到后验


    插曲一下:我认为最伟大的公式当属麦克斯韦方程组,十分的美妙,像一个妙龄少女,闭月羞花,沉鱼落雁,爱了爱了,哈哈哈。。。。
    但是卡尔曼滤波也十分了不起,同样牛逼哄哄。
    说到这儿,摆上大餐,提提文章档次,微分形式的麦克斯韦方程组(核心是两个散度两个旋度):

    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述


    言归正传,转到正台:

    建模(这里讲的数据都是矩阵)

    卡尔曼滤波算法是需要建模的,拿高中物理中典型的例子举例,在二维平面中,跟踪一个质量为m的小球,t0时刻有水平速度v0,只受到重力作用运动,让你找出小球接下来的真实轨迹。你很快想到,很简单,我拿一个相机拍下小球的运动轨迹就好了。理论上没错,事实上拿相机记录也存在一定误差,怎么样来描述这个误差了,我们想到高斯噪声是最常见的噪声类型,那么我们假定相机的观测误差vk满足期望为‘0’协方差矩阵为R的高斯噪声vk~N(0,R),协方差矩阵为R反应了测量噪声的分布情况,矩阵R的迹越大,说明误差越分散,进一步说明测量值置信度越低。
    同时,我们建立一个理想模型来表述它或者叫预测它。假定系统的状态方程为(公式推导可以自己查阅资料):
    在这里插入图片描述
    这个状态方程是根据上一时刻的状态(这里指的是上一时刻的后验估计状态值,嗯,这样说比较准确)和控制变量来推测此刻的状态,由于世界上没有理想的理论模型,wk-1是服从高斯分布的噪声,是预测过程的噪声,它对应了 xk 中每个分量的噪声,期望为 0,协方差为 Q 的高斯白噪声wk-1~N(0,Q),Q即过程激励噪声协方差矩阵Q。

    首先我们需要确定预测的状态变量数量和观测值,比如我们跟踪的是上面的模型,要知道小球的位置,那么我们可以给定预测4个状态值(x,y,vx,vy),观测值就点的坐标(x,y)。我们可以通过k-1次的测量值,来估计第k次的先验估计值,再用卡尔曼增益值将第k次的测量值和第k次的先验估计值做融合,得到第k次的后验估计值,第k次的后验估计值就是我们求出的最优解。

    OpenCV中关于Kalman滤波的结构和函数定义

    Kalman 滤波器状态,具体如何使用请查看opencv官方给的kalman案例。

    typedef struct CvKalman
    {
        int MP;                     /* 测量向量维数 */
        int DP;                     /* 状态向量维数 */
        int CP;                     /* 控制向量维数 */
    
        /* 向后兼容字段 */
    #if 1
        float* PosterState;         /* =state_pre->data.fl */
        float* PriorState;          /* =state_post->data.fl */
        float* DynamMatr;           /* =transition_matrix->data.fl */
        float* MeasurementMatr;     /* =measurement_matrix->data.fl */
        float* MNCovariance;        /* =measurement_noise_cov->data.fl */
        float* PNCovariance;        /* =process_noise_cov->data.fl */
        float* KalmGainMatr;        /* =gain->data.fl */
        float* PriorErrorCovariance;/* =error_cov_pre->data.fl */
        float* PosterErrorCovariance;/* =error_cov_post->data.fl */
        float* Temp1;               /* temp1->data.fl */
        float* Temp2;               /* temp2->data.fl */
    #endif
    
        CvMat* state_pre;           /* 预测状态 (x'(k)): 
                                        x(k)=A*x(k-1)+B*u(k) */
        CvMat* state_post;          /* 矫正状态 (x(k)):
                                        x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) */
        CvMat* transition_matrix;   /* 状态传递矩阵 state transition matrix (A) */
        CvMat* control_matrix;      /* 控制矩阵 control matrix (B)
                                       (如果没有控制,则不使用它)*/
        CvMat* measurement_matrix;  /* 测量矩阵 measurement matrix (H) */
        CvMat* process_noise_cov;   /* 过程噪声协方差矩阵
                                            process noise covariance matrix (Q) */
        CvMat* measurement_noise_cov; /* 测量噪声协方差矩阵
                                              measurement noise covariance matrix (R) */
        CvMat* error_cov_pre;       /* 先验误差计协方差矩阵
                                            priori error estimate covariance matrix (P'(k)):
                                         P'(k)=A*P(k-1)*At + Q)*/
        CvMat* gain;                /* Kalman 增益矩阵 gain matrix (K(k)):
                                        K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)*/
        CvMat* error_cov_post;      /* 后验错误估计协方差矩阵
                                            posteriori error estimate covariance matrix (P(k)):
                                         P(k)=(I-K(k)*H)*P'(k) */
        CvMat* temp1;               /* 临时矩阵 temporary matrices */
        CvMat* temp2;
        CvMat* temp3;
        CvMat* temp4;
        CvMat* temp5;
    }
    CvKalman;
    

    实践(C++)

    目前经典的MOT多目标跟踪算法是sort、deepsort,其中sort算法是先经过Hungarian algorithm(匈牙利算法)进行最大匹配,再经过本文的kalman滤波进行预测。实际项目中我是先用常规(ssd或者yolo)等检测网络,检测出框,,再用最简单的IOU匹配跟踪,再用kalman进行预测。发现加了kalman滤波效果没提升,甚至框子抖动稍微大了一些(打印了kalman预测前后的数值,确认了kalman在工作):原因可能是因为自己使用的是kalman均速模型,而我们神经网络检测出的物体框存在误差(抖动),所以如果每次检测框不准确(抖动过大或者忽大忽小),会导致均速的kalman模型预测也跟着抖动,甚至抖动幅度更大了。切换到加速度kalman模型,发现乱框,加速度kalman模型不能用在目标检测框跟踪上。项目上参考的代码是:https://github.com/Smorodov/Multitarget-tracker
    由于kalman滤波需要建模,所以它适用于跟踪目标符合一定的规律或者趋势,检测框的不确定性太大了,可以试试,一般效果不大,最终还是取决于检测效果的好坏。

    最后收尾:每一个物理学家的必定都是数学家。

    展开全文
  • 用C语言实现的卡尔曼滤波算法+联邦融合算法,适用于一维多传感器的简单应用,内含测试数据,和数据处理后的图表可视化结果,可用于简单的课程设计参考借鉴学习,欢迎提出修改意见。
  • 因为本文主要讲卡尔曼的简单应用,那么讲得就是一维卡尔曼滤波的应用,也就是存在单个变量的情况,而且还是单传感器的输入。所以就直接给出在简单场景下的公式了,而不去推导公式,这样更容易理解和入手。一条最简化...
  • 卡尔曼滤波算法

    2014-04-17 10:22:16
    用MATLAB描述了卡尔曼滤波算法一维二维三维算法,并给出传统和当前算法的基本形态,详情见正文
  • 滤波算法(四)—— 卡尔曼滤波算法

    万次阅读 多人点赞 2020-03-02 11:51:30
    卡尔曼滤波个神奇的滤波算法,应用非常广泛,它是种结合先验经验、测量更新的状态估计算法。 1、状态估计 首先,对于个我们关心的物理量,我们假设它符合下面的规律 其中,为该物理量本周期的实际值...
  • 1iekf 一维的迭代扩展卡尔曼滤波算法 比较简单的算法
  • 实现1维的卡尔曼滤波算法 1.首先完成滤波算法的编程(有参考,不知道作者是谁了,侵权可删) #ifndef KAERMAN_H_ #define KAERMAN_H_ #include <iostream> using namespace std; // 一维滤波器信息结构体 ...
  • 卡尔曼滤波系列——(二)扩展卡尔曼滤波

    万次阅读 多人点赞 2019-04-06 16:33:48
    扩展卡尔曼滤波(Extended Kalman Filter,EKF)是标准卡尔曼滤波在非线性情形下的种扩展形式,它是种高效率的递归滤波器(自回归滤波器)。 EKF的基本思想是利用泰勒级数展开将非线性系统线性化,然后采用...
  • 在此演示中,我们采用了用于跟踪移动对象的卡尔曼滤波算法,并将演示如何从 MATLAB 函数自动生成 C 代码。 自动 MATLAB 到 C 代码生成可用于称为 Embedded MATLAB:registered: 的 MATLAB 语言子集,该子集由 310 多...
  • 卡尔曼滤波算法及其python实现

    千次阅读 多人点赞 2020-01-07 15:20:54
    卡尔曼滤波算法及其python实现算法原理python实现 算法原理 python实现 # KF algorith demo by Leo # 2020.01.06 # ZJG CAMPUS,ZJU import numpy as np import matplotlib.pyplot as plt ''' 生成带噪声的...
  • 深度分析卡尔曼滤波算法原理

    千次阅读 2020-06-24 12:30:01
    关注、星标公众号,不错过精彩内容素材来源:网络编辑整理:strongerHuang说到卡尔曼滤波,想必很多读者都用过,或者听说过,是种应用非常广泛的滤波算法。在网上看了不少与卡尔曼滤波...
  • 关于卡尔曼滤波算法的解析文章数不胜数,对于其强大和超广的适用性这里也不再赘述,这篇文章旨在以极为简单而通俗的语言描述卡尔曼滤波,希望数学小白以及日后的自己也能轻松看懂。 理解这篇文章只需要基本的数学...
  • 扩展卡尔曼滤波算法及仿真实例

    万次阅读 多人点赞 2018-08-17 20:43:24
    在阅读本篇博客之前希望读者已经具备线性卡尔曼滤波器的基础,或者提前研读我的前篇关于线性卡尔曼滤波器的文章:线性卡尔曼滤波算法及示例。下面不说废话,直奔主题了。 、扩展卡尔曼滤波器(EKF)理论基础 ...
  • 卡尔曼滤波算法丝淡淡理解

    千次阅读 2016-12-15 21:17:31
    我觉得卡尔曼滤波算法本质上是个递推反馈算法。它分两部分:时间更新方程和测量状态更新方程。其中,前者负责递推,后者负责反馈(将先验估计和新的测量变量结合,以构造改进后的后验估计)。 时间更新方程
  • 提出了种基于卡尔曼滤波技术的提高双飞秒激光绝对距离测量精度的方法, 即通过建立卡尔曼滤波状态空间模型得到测量结果的最优状态估计, 大幅度降低随机过程引入的测量精度损失, 同时可以得到速度信息估计值。...
  • 目录、背景详解二、卡尔曼滤波(Kalman)原理参考文献 、背景详解 只要是存在不确定信息的动态系统,卡尔曼滤波...卡尔曼滤波(Kalman)无论是在单目标还是多目标领域都是很常用的算法,我们将卡尔曼滤波看做
  • 卡尔曼滤波系列——()标准卡尔曼滤波

    万次阅读 多人点赞 2019-03-03 16:03:58
    卡尔曼滤波(Kalman Filter)是种利用线性系统状态方程,利用对系统的观测数据,对系统状态进行最优估计的算法。由于观测数据受到系统中的噪声和干扰的影响,所以系统状态的估计过程也可看作是滤波过程。应用场景...
  • 提出了种利用卡尔曼滤波剔除动态野值改进的算法,该方法提高了卡尔曼滤波器在机载环境下的跟踪能力,由于降低了状态方程的数,滤波的实时性得到提高,经试飞数据验证,由该算法滤波后无源北斗系统的定位精度可以...
  • 为解决四元数在无迹卡尔曼滤波(UKF)算法中的应用问题,提出了种基于四元 数的状态扩无迹卡尔曼滤波(Q--AUKF)算法.该算法将系统噪声增广到状态向量中,解决了乘性四元数噪声无法 进行向量意义下四则运算的问题.针对...
  • 卡尔曼滤波算法及C语言实现_源代码

    万次阅读 多人点赞 2019-01-16 20:39:20
    卡尔曼滤波理论很容易就可以在MATLAB软件环境下实现,但是,实际的硬件板子上还是需要C语言,当然可以自动代码生成,还有种就是直接手动编写C语言。

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 2,889
精华内容 1,155
关键字:

一维卡尔曼滤波算法