精华内容
下载资源
问答
  • 拓展卡尔曼滤波由线性卡尔曼滤波发展而来。虽然线性卡尔曼滤波存在只能处理线性系统的局限性,但是拓展卡尔曼滤波解决了这一问题,而且在非线性系统中经常取得较好的吻合。当然拓展卡尔曼滤波比线性卡尔曼滤波实现...

          拓展卡尔曼滤波由线性卡尔曼滤波发展而来。虽然线性卡尔曼滤波存在只能处理线性系统的局限性,但是拓展卡尔曼滤波解决了这一问题,而且在非线性系统中经常取得较好的吻合。当然拓展卡尔曼滤波比线性卡尔曼滤波实现起来也更加困难,目前网上的资料也不是很多,特别是关于MATLAB的实现资料很少。

     

    注意:1、本文中,时间k作为下标而不是放在括号内。

                2、本文作为拓展卡尔曼滤波分析,默认读者已经至少简单了解线性卡尔曼滤波相关知识。

               3、拓展卡尔曼滤波公式繁多,具有极强劝退效果。本文也不能免俗,唯希望读者可以跟着步骤走。如果难理解,希望多读几遍。作者虽自觉水平有限,但亦觉得比网上大多文章透彻清晰。

     

    • 关于线性卡尔曼滤波(简单带过)。

    下面是线性卡尔曼滤波的五个方程。如果有读者连线性卡尔曼也不熟悉,建议bilibili看一些讲解视频,我个人觉得vic讲的很不错,他在视频后面编写了线性卡尔曼的MATLAB实现程序。

    • 拓展卡尔曼滤波

    这是本文的重头戏,也将会花最大篇幅来详细讲解拓展卡尔曼滤波。

    拓展卡尔曼滤波困难之处就在于线性化的过程。本例以二维小车运动为例来进行讲解。(本例中状态变化仍为线性,但是观测是非线性)

     

    1、 首先生成一组数据作为小车真实运动轨迹。

    物理模型基本假定:匀速运动,但存在随机扰动(将很多物理因素的共同作用归结为数学模型中的随机扰动)作为加速度。

     

    建立状态向量

                                                                                   s = \begin{pmatrix}x \\ \dot{x} \\ y \\ \dot{{y}} \end{pmatrix}(其中 \dot{x} 即是 x' 即为速度v_{x},y方向同理)

    根据高中物理知识可得状态转移方程(\ddot{x}即为x'',即加速度扰动,y方向同理)

    写成矩阵形式即为                                                 s_{k}=A*s_{k-1}+G*u_{k-1},u_{k-1}=\begin{pmatrix}\ddot{{x}} \\ \ddot{y} \end{pmatrix}

    其中

    由此即可得到小车的运动轨迹。

     

    2、然后开始进行EKF滤波估计

    先理解一下EKF的方程(其中为符合雅克比矩阵习惯,式6.25 C_{k}更常写作H_{k}

    和线性卡尔曼滤波一样,拓展卡尔曼滤波同样分为两个部分(预测和测量更新)。但是拓展卡尔曼滤波处理的是非线性系统,所以不能以矩阵相乘必须以函数形式来更一般的表示上一时刻到现在时刻的变化。本例中状态向量是s而非x,观测向量是(r,b)而非y。并且本例中的状态变化是线性的,但是观测是非线性的。

    因为状态变化是线性的,所以预测部分非常简单

    在循环中

       s_ = A_*s(:,i-1);

       P_ = A_*P*A_'+G_*Q_*G_';

    如上两行就可以吻合拓展卡尔曼滤波方程的预测部分。

     

    重点是观测更新部分,函数h是非线性的,所以无法直接用矩阵运算来实现观测更新。但是我们有办法把非线性转化为线性。

    在大学课程《高等数学》中我们学过,非线性函数y=h(x)可通过泰勒公式在点(x0,y0)处展开为泰勒级数:

    忽略掉二次项及以上,即可留下线性方程,来代替变化函数h(x)。

    将非线性函数h(x)拓展到多维,即求各个变量的偏导数(本例即为二维小车的运动),即:

    对x求偏导数所对应的这一项被称为雅可比(Jacobian)式。

     

    我们可以直接雅克比矩阵结论:

    在本例中h(x):

    由求偏导公式可以得出本例雅克比矩阵为:

    P_{x},P_{y}即为x,y,r\sqrt{x^2+y^2}

    有了雅可比矩阵,即可由 式6.27 求得卡尔曼增益M_{k}

     

    另外单独说一下式6.28。

    式6.28 红色部分即为最小均方误差MMSE。y_{k}是观测值(本例在第一部分生成),h(\hat{x}_{k|k-1},0)是预测部分 式6.23 得到的估计值进行h变换结果,换句话说就是把式6.23得到的估计值带入h(x),生成新的观测向量r_{k},b_{k}。再用观测值y_{k}相减从而得到MMSE。

    式6.29 非常普通,简单带入即可。

     

    • 本例初始值即环境参数设置

    为防止某些小白看到接近终点放弃,多嘴一句MVN是多维正态分布,大写I是单位矩阵。2,3部分是扰动服从MVN分布。这一点在代码中体现更加直观。

     

    • MATLAB代码
    clear;
    close all;
    
    %% 设置初始值和环境
    MarkerSize = 15; FontSize = 25; LineWidth = 2;
    
    S(:,1) = [10;-0.2;-5;0.2];               %大写S矩阵保存真实值
    no_noise(:,1) = [10;-0.2;-5;0.2];        %no_noise保存理想值
    s(:,1) = [5;0;5;0];                      %小写s矩阵保存EKF估计值
    T = 1;
    
    A = [1 T 0 0; 0 1 0 0; 0 0 1 T; 0 0 0 1];%状态转移矩阵
    G = [1/2*T^2 0; T 0; 0 1/2*T^2; 0 T];    %加速度叠加矩阵
    
    %% 生成真实值和观测值rk bk
    for i=1:500
        u(:,i) = mvnrnd([0,0],[0.0001,0;0,0.0001])';                        %随机噪声
        t(:,i) = mvnrnd([0,0],[0.01,0;0,0.01])';                            %测量噪声
        YY(:,i) = [sqrt(S(1,i)^2+S(3,i)^2);atan2(S(3,i),S(1,i))]+t(:,i);    %生成观测值
        S(:,i+1) = A*S(:,i)+G*u(:,i);                                       %生成真实值
        no_noise(:,i+1) = A*no_noise(:,i);                                  %生成理想值
        
    end
    
    figure(1)
    % 取消注释可以绘制理想轨迹
    % plot(no_noise(1,2: end), no_noise(3,2: end),'r', 'LineWidth',LineWidth, 'MarkerSize',MarkerSize)
    % axis equal
    % hold on                     %绘制理想轨迹
    
    plot(S(1,2: end), S(3,2: end),'-', 'LineWidth',LineWidth, 'MarkerSize',MarkerSize)
    axis equal
    hold on                     %绘制真实轨迹
    
    A_ = A; G_ = G;
    P = 100*eye(4);
    
    Q_ = 1e-4*eye(2);
    R = diag([0.01, 0.01]);     %观测噪声协方差矩阵
    
    
    %% 根据卡尔曼方程来进行滤波
    for i = 2:500
        %预测
       s_ = A_*s(:,i-1);
       P_ = A_*P*A_'+G_*Q_*G_'; 
        %更新
       px = s_(1); py = s_(3); 
            % 雅克比矩阵(第一次计算雅克比矩阵尝试)
            %    H = [px/(px^2+py^2)^(1/2) py/(px^2+py^2)^(1/2) 0 0;
            %         -py/(px^2+py^2)      px/(px^2+py^2)       0 0;
            %         py*(vx*py-vy*px)/(px^2+py^2)^(3/2) px*(vy*px-vx*py)/(px^2+py^2)^(3/2) px/(px^2+py^2)^(1/2) py/(px^2+py^2)^(1/2)];
       H = [px/(px^2+py^2)^(1/2) 0 py/(px^2+py^2)^(1/2) 0;
            -py/(px^2+py^2)      0 px/(px^2+py^2)       0];  %雅克比矩阵
       Mk = P_*H'/(H*P_*H'+R);          %卡尔曼增益
       y = [(s_(1)^2+s_(3)^2)^(1/2);atan2(s_(3),s_(1))];
       s(:,i) = s_+Mk*(YY(:,i)-y);
       P = P_ - Mk*H*P_;
       
    end
    
    plot(s(1,2: end), s(3,2:end),':', 'LineWidth',LineWidth, 'MarkerSize',MarkerSize)
    % legend('理想轨迹','真实轨迹', 'EKF估计')
    legend('真实轨迹', 'EKF估计')
    grid on; ax = gca; ax.FontSize = FontSize;
    
    
    %% 绘制rk和bk
    r_k = YY(1,2:end); b_k = YY(2,2:end);
    figure(2)
    subplot(211)
    plot(r_k);
    legend('r_k');
    subplot(212)
    plot(b_k,'r');
    legend('b_k');
    
    %% 绘制rk,bk计算轨迹和真实轨迹对比
    figure(3)
    xr = r_k.*cos(b_k);
    yr = r_k.*sin(b_k);
    plot(yr,xr)
    hold on
    plot(S(1,2: end), S(3,2: end),'-', 'LineWidth',LineWidth, 'MarkerSize',MarkerSize)
    legend('根据r_k,b_k计算轨迹','真实轨迹');

     

    展开全文
  • 利用拓展卡尔曼滤波方法,基于WiFi信号RSSI实现室内定位和轨迹追踪,同时绘制轨迹图,相关数据存储在安卓数据库中,另外含有路径损耗估计算法和最小二乘算法,适合室内定位领域的研究生和感兴趣的读者下载。
  • SLAM学习 | 用拓展卡尔曼滤波(EKF)做小车轨迹跟踪 1 卡尔曼滤波 2 拓展卡尔曼滤波 3 小车轨迹跟踪 (我太难了,寒假填坑) 概要: 这是一个小demo,用拓展卡尔曼滤波(EKF)实现小车的轨迹跟踪(trajectory ...


    概要: 这是一个小demo,用拓展卡尔曼滤波(EKF)实现小车的轨迹跟踪(trajectory tracking)。
    关键字: 关键词1; 关键词2

    1 卡尔曼滤波

    2 拓展卡尔曼滤波

    3 小车轨迹跟踪

    (我太难了,寒假填坑)

    展开全文
  • 卡尔曼滤波与拓展卡尔曼滤波

    千次阅读 2018-02-19 23:27:46
    前言: 从上个世纪卡尔曼滤波理论被提出,卡尔曼滤波在控制论与信息论的...为了得出准确的下一时刻状态真值,我们常常使用卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波、粒子滤波等等方法,这些方法在姿态解算、轨迹规
    【[转自](http://blog.csdn.net/qq_18163961/article/details/52505591)】

    前言:

    从上个世纪卡尔曼滤波理论被提出,卡尔曼滤波在控制论与信息论的连接上做出了卓越的贡献。为了得出准确的下一时刻状态真值,我们常常使用卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波、粒子滤波等等方法,这些方法在姿态解算、轨迹规划等方面有着很多用途。卡尔曼滤波的本质是参数化的贝叶斯模型,通过对下一时刻系统的初步状态估计(即状态的先验估计)以及测量得出的反馈相结合,最终得到改时刻较为准确的的状态估计(即状态的后验估计),其核心思想即为预测+测量反馈,而这两者是通过一个变化的权值相联系使得最后的状态后验估计无限逼近系统准确的状态真值,这个权值即为大名鼎鼎的卡尔曼增益。可以说,卡尔曼滤波并不与传统的在频域的滤波相似,而是一种在时域的状态预测器,这就省去了时域频域的变换的步骤,而这种状态预测器不仅仅在工程上有很广的应用,在金融方面例如股票的走势等等方面也可以有很多的应用。

    一直以来,卡尔曼滤波相关的资料较少,很多人觉得很难,因此写下这篇博客来帮助大家更好的理解卡尔曼滤波的运作原理。

    2017年2月11日  by 回忆不能已 

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

    一、卡尔曼滤波的定义

    卡尔曼滤波的定义:

    种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响,所以最优估计也可看作是滤波过程。

    1.1  线性系统状态方程

    首先我们来看什么是线性系统状态方程,这个名词包含线性系统、状态这两个特征。

    线性系统

    状态空间描述(内部描述):基于系统内部结构,是对系统的一种完整的描述。

    状态方程

          描述系统状态变量间或状态变量与系统输入变量间关系的一个一阶微分方程组(连续系统)或一阶差分方程组(离散系统),称为状态方程

    1.2  观测数据

    观测数据代表传感器采集的实际数据,可能存在着或多或少的误差,例如陀螺仪的积分误差等。

    1.3  最优估计

    最优估计指的是使经过KF算法解算的数据无限接近于真实值的估计,用数学表述即为后验概率估计无限接近于真实值。

    二、卡尔曼滤波算法流程

    卡尔曼滤波算法核心思想在于预测+测量反馈,它由两部分组成,第一部分是 线性系统状态预测方程,第二部分是 线性系统观测方程。

    2.1  线性系统状态预测方程

    A :表示状态转移系数矩阵,n×n

    B :表示可选的控制输入的增益矩阵

    Q :表示过程激励噪声的协方差矩阵

    上图为线性系统状态预测方程的表达式,假设过程激励噪声满足高斯分布。

    2.2  线性系统观测方程

    H :表示量测系数矩阵,m×n 阶矩阵

    R :表示测量噪声协方差矩阵

    上图为线性系统观测方程表达式,假设测量噪声矩阵满足高斯分布

    随机信号 wkvk分别表示过程激励噪声和观测噪声。Kf算法中假设它们为相互独立,正态分布的白色噪声。

    预测+测量反馈进行后续的估计,那kf算法到底是如何利用预测状态与测量的反馈进行最优估计的呢?这部分我们将在后续的模型及数学推导来详细讲解

    2.3  扩展卡尔曼滤波EKF流程

    EKF的基本思想是将非线性系统线性化然后进行卡尔曼滤波,因此EKF是一种伪非线性的卡尔曼滤波。实际中一EKF应用广泛

    但EKF存在一定的局限性:

    其一是当强非线性时EKF违背局部线性假设,Taylor展开式中被忽略的高阶项带来大的误差时,EKF算法可能会使滤波发散;

    另外,由于EKF在线性化处理时需要用雅克比(Jacobian)矩阵,其繁琐的计算过程导致该方法实现相对困难。所以,在满足线性系统、高斯白噪声、所有随机变量服从高斯(Gaussian)分布这3个假设条件时,EKF是最小方差准则下的次优滤波器,其性能依赖于局部非线性度。

    以PIXHAWK飞控代码为例说明:

    1、非线性系统线性化

    EKF对非线性函数的Taylor展开式进行一阶线性化截断,忽略其余高阶项,从而将非线性问题转化为线性,可以将卡尔曼线性滤波算法应用于非线性系统中多种二阶广义卡尔曼滤波方法的提出及应用进一步提高了卡尔曼滤波对非线性系统的估计性能

    2、二阶滤波方法考虑了Taylor级数展开的二次项,因此减少了由于线性化所引起的估计误差,但大大增加了运算量

    3pixhawk开源项目使用的是一阶EKF滤波,因为二到三阶滤波计算量较大如果飞行器的处理速度能跟上,可以考虑2-3阶滤波。

    扩展卡尔曼滤波的状态预测方程以及观测方程与一般的卡尔曼滤波略有不同:

    KF与EKF的联系如下图所示:

    三、卡尔曼滤波算法模型


    3.1  卡尔曼滤波算法应用前提

    (1)系统是可观测
    (2)系统为线性系统,能够写出下一时刻的状态预测方程

    (3)假设系统的噪声统计特性可观测

    应用中大多数情况下并不知道建模噪声和观测噪声的统计特性,一般假定为零均值,方差的值通过测试或是经验丰富的开发人员设置得到,噪声一般直接简化假设为高斯白噪声

    3.2  预先的一些数学知识准备

    先验估计:先验状态估计是根据系统过程原理或者经验得到的估计值,实际应用中可以通过传感器数据去预测下一时刻的数据

    后验估计:验状态估计是结合之前的先验状态估计值,再加权测量值得到一个理论上最接近真实值的结果

    协方差:协方差用于衡量两个变量的总体误差。而方差是协方差的一种特殊情况,即当两个变量是相同的情况

    均方差:它是“误差”的平方期望值

    矩阵的迹:矩阵对角线上的所有元素之和称之为矩阵的迹


    协方差矩阵的主对角线元素分别为两个变量的方差

    最小均方差估计就是指估计参数时要使得估计出来的模型和真实值之间的误差平方期望值最小。

    3.3  模型讲解

    实际的状态模型如下所示:

    随机信号wv分别表示过程激励噪声和观测噪声

    假设它们为相互独立,正态分布的白色噪声

    图片中蓝色标记的为已知的矩阵和信息,卡尔曼滤波的目的就是利用这些已知的信息估计离散时间过程的状态变量。为什么是估计呢?因为有噪声、 存在。而信息就这么多,我们要做的就是利用这手头的信息尽量准确的估计出离散过程的状态变量,从而随时掌握系统的运行状态和变化。

    举个日常生活的例子:

    假设你有两个传感器,测的是同一个信号。可是它们每次的读数都不太一样,怎么办?取平均。

    再假设你知道其中贵的那个传感器应该准一些,便宜的那个应该差一些。那有比取平均更好的办法吗?加权平均。

    怎么加权?假设两个传感器的误差都符合正态分布,假设你知道这两个正态分布的方差,用这两个方差值,(此处省略若干数学公式),你可以得到一个“最优”的权重。

    接下来,重点来了:

    假设你只有一个传感器,但是你还有一个数学模型。模型可以帮你算出一个值,但也不是那么准。怎么办?把模型算出来的值,和传感器测出的值,(就像两个传感器那样),取加权平均。OK,最后一点说明:你的模型其实只是一个步长的,也就是说,知道x(k),我可以求x(k+1)。问题是x(k)是多少呢?答案:x(k)就是你上一步卡尔曼滤波得到的、所谓加权平均之后的那个、对x在k时刻的最佳估计值。于是迭代也有了。

    卡尔曼滤波的模型:

    带卡尔曼滤波器的系统方框图,图中的上半部分是实际的离散时间过程(有噪声的存在),下面部分是卡尔曼滤波器。卡尔曼滤波器通过利用蓝色标记的可用信息对系统的状态变量进行估计,得出粉色标记的方框,而这个得以实现的关键就是红色标记出的反馈信息的利用,即设置怎样的反馈增益矩阵K使状态变量在某种意义下最准确,最接近真实系统状态。

    我们认定是预测(先验)值,是估计值,为测量值的预测,在下面的推导中,请注意估计和预测两者的区别,不混为一谈。由一般的反馈思想我们得到估计值:

    其中括号里面的公式称之为残差,也就是预测的和你实际测量值之间的差距。如果这项等于0,说明预测和测量出的完全吻合。

    关键就是求取这个K。这时最小均方差就起到了作用。

    顺便在这里回答为什么噪声必须服从高斯分布,在进行参数估计的时候,估计的一种标准叫最大似然估计,它的核心思想就是你手里的这些相互间独立的样本既然出现了,那就说明这些样本概率的乘积应该最大(概率大才出现嘛)。如果样本服从概率高斯分布,对他们的概率乘积取对数ln后,你会发现函数形式将会变成一个常数加上样本最小均方差的形式。因此,看似直观上很容易理解的最小均方差理论上来源就出于那里。


    我们想得到最优的估计,那么必须知道k时刻的后验估计协方差与k时刻的先验估计协方差。

    k时刻的后验估计协方差表达式为

    红色方框里面表达式在前面提过,即

    k时刻的先验估计协方差表达式为

    我们把k时刻的后验估计协方差矩阵的迹即为T[Pk]

    Pk对角线元素的和即为均方差。

    x^k¯:表示k时刻先验状态估计值,这是算法根据前次迭代结果(就是上一次循环的后验估计值)做出的不可靠估计。x^k、x^k1:分别表示k时刻、k-1时刻后验状态估计值,也就是要输出的该时刻最优估计值,这个值是卡尔曼滤波的结果。 A 表示状态转移矩阵,是n×n阶方阵,它是算法对状态变量进行预测的依据,状态转移矩阵如果不符合目标模型有可能导致滤波发散,它的确定请参看第二节中的举例

     B 表示可选的控制输入u∈Rl的增益,在大多数实际情况下并没有控制增益

    uk1:表示k-1时刻的控制增益,一般没有这个变量,可以设为0。

    P^k¯ :表示k时刻的先验估计协方差,这个协方差矩阵只要确定了一开始的P^0,后面都可以递推出来,而且初始协方差矩阵P^0只要不是为0,它的取值对滤波效果影响很小,都能很快收敛。

     P^k P^k1 :分别表示k时刻、k-1时刻的后验估计协方差,是滤波结果之一。

    Q:表示过程激励噪声的协方差,它是状态转移矩阵与实际过程之间的误差。这个矩阵是卡尔曼滤波中比较难确定的一个量,一般有两种思路:一是在某些稳定的过程可以假定它是固定的矩阵,通过寻找最优的Q值使滤波器获得更好的性能,这是调整滤波器参数的主要手段,Q一般是对角阵,且对角线上的值很小,便于快速收敛;二是在自适应卡尔曼滤波(AKF)中Q矩阵是随时间变化的。

    Kk 表示卡尔曼增益,是滤波的中间结果

    zk 表示测量值,是m阶向量

    H表示量测矩阵,是m×n阶矩阵,它把m维测量值转换到n维与状态变量相对应

    R:表示测量噪声协方差,它是一个数值,这是和仪器相关的一个特性,作为已知条件输入滤波器。需要注意的是这个值过大过小都会使滤波效果变差,且R取值越小收敛越快,所以可以通过实验手段寻找合适的R值再利用它进行真实的滤波。

    最小均方差求解

    均方差对未知量K求导,令导函数等于0,就能确定卡尔曼增益K的值,完成模型的最小均方差估计,从而使后验估计的误差很小,更加逼近状态的真值

    最小均方差确定卡尔曼增益的表达式

    H:表示量测矩阵

    R:表示测量噪声协方差,和仪器相关

    3.4  卡尔曼增益的分析

    先验估计误差较大,我们需要更加信任测量反馈,模型自动加大卡尔曼增益来进行更准确的估计

    先验估计没有任何误差,我们只需要通过预测就能够得到准确的后验估计,无需测量的反馈,模型自动将测量反馈的权值设为0

    3.5模型的迭代过程


    3.5.1时间更新方程

    A :表示状态转移矩阵,n×n

    B :表示可选的控制输入的增益矩阵

    Q :表示过程激励噪声的协方差矩阵

    3.5.2 状态更新方程

    H :表示量测矩阵,m×n 阶矩阵

    R :表示测量噪声协方差矩阵

    3.5.3  迭代示意图


    下篇博客会详细分析EKF的模型,对其非线性系统的建模进行讲解,欢迎大家指出博客中的错误!

                    </div>
    
    展开全文
  • 拓展卡尔曼滤波.pdf

    2020-07-19 22:39:32
    为了校正这种误差,我们通常会引入额外的传感器信息。例如,对上图来说,引入GPS定位信息就是一个不错...那么我们该如何综合利用这些信息(传感器信息融合)来估计机器人当前的位置信息呢,这里就要用到卡尔曼滤波了。
  • 拓展卡尔曼滤波学习(python源码)

    千次阅读 2020-09-10 11:42:11
    拓展卡尔曼滤波的逐步理解与实现 这个文章讲的非常不错。配套代码实现文章。 【机器人位置估计】卡尔曼滤波的原理与实现 本文主要是针对两篇文章的基础上做笔记和记录学习过程。 一、基本模型 1.1 机器人小M ...

    拓展卡尔曼滤波的逐步理解与实现

    这个文章讲的非常不错。配套代码实现文章。

    【机器人位置估计】卡尔曼滤波的原理与实现

    本文主要是针对两篇文章的基础上做笔记和记录学习过程。

    一、基本模型

    1.1 机器人小M

    image

    现在小M只具有一个物理量-位移x,也就是一维卡尔曼/

    1.2 位移状态预测值

    image 估计值自身会由于运动模型预测不准确而导致预测误差,由误差值得到的状态值也是存在误差的,如果以存在误差的状态值继续预测下一个状态值,误差会存在累计的问题。需要引入测量量进行修正。

    1.3 传感器测量值

    image

    同样,我们使用的传感器也是存在测量精度的问题,无法完全的等于实际物理量的大小。

    所以也就提出了卡尔曼老人解决这个问题的思路。既然测量值和估计值都存在一定的误差值,那么将两组数据进行融合,谁更准便将其权重提高。

    1.4 卡尔曼核心思想

    image

    确定那个数据的置信度更高,这就牵扯到了预测误差的准确值Pt,所以需要通过一定的方式对Pt进行计算,随着整个过程进行一定的调整和变化。

    1.5 计算 预测误差准确值

    image

    没有绝对准确的运动模型,也没有绝对准确的传感器

    1.6 卡尔曼滤波公式

    image

    预测就是利用上一步的状态准确值和预测误差准确值,结合运动模型得到下一步的状态估计值和预测误差估计值

    更新就是融入传感器信息,计算增益系数,将估计值再次转换为准确值。

    2.拓展卡尔曼滤波

    2.1 加入控制量

    image

    机器人的移动、转动都是人为引入的一种控制量。

    2.2 观测系数

    image

    这里给的例子很明确的表现观测系数c的作用。引入观测洗漱势必会对预测误差产生一定比例的影响,发生一定变化。 image

    2.3 拓展卡尔曼滤波公式

    image

    多维卡尔曼滤波

    imageimageimage

    3.使用实例

    image

    import numpy as np
    import math
    import matplotlib.pyplot as plt
    
    if __name__=="__main__":
        ## 1.设计一个匀加速直线运动,以观测此运动
        X_real = np.mat(np.zeros((2, 100))) # 空矩阵,用于存放真实状态向量
        X_real[:, 0] = np.mat([[0.0], # 初始状态向量
                               [1.0]])
        a_real = 0.1# 真实加速度
        F = np.mat([[1.0, 1.0], # 状态转移矩阵
                    [0.0, 1.0]])
        Q = np.mat([[0.0001, 0.0], # 状态转移协方差矩阵,我们假设外部干扰很小
                    [0.0, 0.0001]])
        B = np.mat([[0.5], # 控制矩阵
                    [1.0]])
        for i in range(99):
            X_real[:, i + 1] = F * X_real[:, i] + B * a_real # 计算真实状态向量
        X_real = np.array(X_real)
        fig = plt.figure(1)
        plt.grid()
        plt.title('real displacement')
        plt.xlabel('k (s)')
        plt.ylabel('x (m)')
        plt.plot(X_real[0, :])
        plt.show()
        fig = plt.figure(2)
        plt.grid()
        plt.title('real velocity')
        plt.xlabel('k (s)')
        plt.ylabel('v (m/s)')
        plt.plot(X_real[1, :])
        plt.show()
        X_real = np.mat(X_real)
    

    image注意到这里预测值是非常准的,在置信度比例方面会提高的

    image

        ## 2.建立传感器观测值
        z_t = np.mat(np.zeros((2, 100))) # 空矩阵,用于存放传感器观测值
        H = np.mat(np.zeros((2, 2)))
        H[0, 0], H[1, 1] = -1.0, -1.0
        noise = np.mat(np.random.randn(2,100)) # 加入位移方差为1,速度方差为1的传感器噪声
        R = np.mat([[1.0, 0.0], # 观测噪声的协方差矩阵
                    [0.0, 1.0]])
        for i in range(100):
            z_t[:, i] = H * X_real[:, i] + noise[:, i]
        z_t = np.array(z_t)
        fig = plt.figure(3)
        plt.grid()
        plt.title('sensor displacement')
        plt.xlabel('k (s)')
        plt.ylabel('x (m)')
        plt.plot(z_t[0, :])
        plt.show()
        fig = plt.figure(4)
        plt.grid()
        plt.title('sensor velocity')
        plt.xlabel('k (s)')
        plt.ylabel('v (m/s)')
        plt.plot(z_t[1, :])
        plt.show()
        z_t = np.mat(z_t)
    

    image

    卡尔曼滤波实现

        ## 3.执行线性卡尔曼滤波
        Q = np.mat([[1.0, 0.0], # 状态转移协方差矩阵,我们假设外部干扰很小,
                    [0.0, 1.0]])# 转移矩阵可信度很高
        # 建立一系列空序列用于储存结果
        X_update = np.mat(np.zeros((2, 100)))
        P_update = np.zeros((100, 2, 2))
        X_predict = np.mat(np.zeros((2, 100)))
        P_predict = np.zeros((100, 2, 2))
        P_update[0, :, :] = np.mat([[1.0, 0.0], # 状态向量协方差矩阵初值
                                    [0.0, 1.0]])
        P_predict[0, :, :] = np.mat([[1.0, 0.0], # 状态向量协方差矩阵初值
                                     [0.0, 1.0]])
        for i in range(99):
            # 预测
            X_predict[:, i + 1] = F * X_update[:, i] + B * a_real
            P_p = F * np.mat(P_update[i, :, :]) * F.T + Q
            P_predict[i + 1, :, :] = P_p
            # 更新
            K = P_p * H.T * np.linalg.inv(H * P_p * H.T + R) # 卡尔曼增益
            P_u = P_p - K * H * P_p
            P_update[i + 1, :, :] = P_u
            X_update[:, i + 1] = X_predict[:, i + 1] + K * (z_t[:, i + 1] - H * X_predict[:, i + 1])
        X_update = np.array(X_update)   
        X_real = np.array(X_real) 
        fig = plt.figure(5)
        plt.grid()
        plt.title('Kalman predict displacement')
        plt.xlabel('k (s)')
        plt.ylabel('x (m)')
        plt.plot(X_real[0, :], label='real', color='b')
        plt.plot(X_update[0, :], label='predict', color='r')
        plt.legend()
        plt.show()
        fig = plt.figure(6)
        plt.grid()
        plt.title('Kalman predict velocity')
        plt.xlabel('k (s)')
        plt.ylabel('v (m/s)')
        plt.plot(X_real[1, :], label='real', color='b')
        plt.plot(X_update[1, :], label='predict', color='r')
        plt.legend()
        plt.show()
        X_update = np.mat(X_update)
        X_real = np.mat(X_real)
    

    image 这里kt取到了很低,非常不信任传感器测量到的值。

    卡尔曼滤波将存在误差的传感器信号和状态向量很好的融合,较好地预测出了真实地机器人运动状态。

    展开全文
  • 卡尔曼滤波系列——(二)扩展卡尔曼滤波

    万次阅读 多人点赞 2019-04-06 16:33:48
    扩展卡尔曼滤波(Extended Kalman Filter,EKF)是标准卡尔曼滤波在非线性情形下的一种扩展形式,它是一种高效率的递归滤波器(自回归滤波器)。 EKF的基本思想是利用泰勒级数展开将非线性系统线性化,然后采用...
  • 卡尔曼滤波(4):拓展卡尔曼滤波

    千次阅读 2018-04-05 11:42:09
    然而当我们回过头重新检视卡尔曼滤波,发现它用到了大量的线性代数运算,换句话说,只有满足线性的系统才能使用离散卡尔曼滤波。然而在实际工程应用中,系统内总是存在着非线性关系,例如平方关系,三角函数关系,...
  • 一些最著名和有趣的卡尔曼滤波应 用就是处理这些情况的。 将期望和方差线性化的卡尔曼滤波器称作扩展卡 尔曼滤波器(Extended Kalman Filter), 简称EKF。 同泰勒级数类似, 面对非线性关系时, 我们可以通过求...
  • 有关使用EKF进行姿态解算的各公式、公式的推导过程已经各雅各比矩阵的计算过程,上面有我自己的学习笔记,希望多交流!
  • 卡尔曼滤波的缺点就是需要精确的模型,所以由此产生了自校正卡尔曼滤波 自适应卡尔曼滤波 所以都是在解决问题中产生 下面拍自《多传感器加权观测融合kalman滤波理论》 下面拍自《现代信息融合技术在组合...
  • 一、卡尔曼基本思路(以温度测量为例) 卡尔曼滤波,最最容易理解的讲解.找遍网上就这篇看懂了 二、卡尔曼原理推导 https://blog.csdn.net/young_gy/article/details/78177291 三、拓展卡尔曼滤波
  • 卡尔曼滤波与扩展卡尔曼滤波

    千次阅读 2018-04-22 10:51:43
    卡尔曼滤波(Kalman filtering)一种线性系统状态方程,对输入输出的观测数据进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响,所以最优估计也可看作是滤波过程。故求解最优估计的算法就称之为...
  • 比较了扩展卡尔曼滤波定位误差和卡尔曼定位误差的区别
  • 为了得出准确的下一时刻状态真值,我们常常使用卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波、粒子滤波等等方法,这些方法在姿态解算、轨迹规划等方面有着很多用途。卡尔曼滤波的本质是参数化的贝叶斯模型,通过对下...
  • 前段时间由于项目关系需要实现基于卡尔曼滤波的姿态解算,也就是说融合加速度计、陀螺仪及磁罗盘进行AHRS姿态解算,得到机器人的姿态角。 本文的学习需要有一定的卡尔曼滤波器基础,可以参考白巧克力亦唯心的卡尔曼...
  • 卡尔曼滤波存在着一个非常大的局限性——它仅能对线性的处理模型和测量模型进行精确的估计,在非线性的场景中并不能达到最优的估计效果。 所以之前为了保证我们的处理模型是线性的,我们上一节中使用了恒定...
  • 两篇比较好的文章,解释的比较详细,一个作者的 https://zhuanlan.zhihu.com/p/41767489 https://zhuanlan.zhihu.com/p/42364563
  • 扩展卡尔曼滤波详解

    千次阅读 2020-11-15 18:12:26
    卡尔曼滤波详解讲解的基本的卡尔曼滤波算法是通过一个线性随机差分方程来估计当前状态,但是如果状态估计关系以及测量关系使非线性的怎么办,而且在实际使用中大部分的问题都是非线性的,所以提出了扩展卡尔曼滤波...
  • 卡尔曼滤波及Matlab实现

    万次阅读 多人点赞 2018-08-07 01:19:53
    卡尔曼滤波原理浅析 一、什么是卡尔曼滤波  滤波是从信号中提取有用信息的过程,比如从电信号中提取有用的频谱分量,从观测到的物体轨迹中提取位置信息,滤除图像信号中的噪声等。卡尔曼滤波是一种有效的滤波...
  • 自适应卡尔曼滤波

    2018-12-14 17:24:59
    能运行的简单的自适应卡尔曼滤波程序,多维的滤波可在其上扩展。
  • 目录KFEKF KF https://blog.csdn.net/u012554092/article/details/78290223 https://blog.csdn.net/young_gy/article/details/78177291 EKF https://blog.csdn.net/Young_Gy/article/details/78468153
  • 文章目录卡尔曼滤波拓展卡尔曼滤波正弦波滤波 卡尔曼滤波 卡尔曼滤波是一个线性状态估似算法,对高斯噪声有良好的滤波效果,也可做多传感器融合算法,如九轴。具体推导网络上,网络上很多。珠玉在前我就不过多赘述了...
  • 卡尔曼滤波初识

    2021-06-03 12:28:41
    最近偶然接触到了卡尔曼滤波这个概念,先来简单地学习它的基本理念吧~卡尔曼滤波用于预测,既能包含数据的时序信息,也可以结合当前时刻的观测信息。 卡尔曼递推算法根据前一个时刻状态的估计值和当前时刻的观测...
  • %滤波周期 T_go=10;%滤波时间 N=T_go/T;%观测次数 t=0:T:T_go-T;%假定输出序列(供画图用) x=zeros(2,N); z=zeros(2,N); x(:,1)=[1;0];%真值初始值 mu=[0;0]; Q=[0.01,0;0,0.0001]; R=[0.1,0;0,0.1]; rng(1); w=...
  • 扩展卡尔曼滤波

    千次阅读 2021-04-05 19:58:33
    扩展卡尔曼滤波(Extended Kalman Filter,EKF)是标准卡尔曼滤波在非线性情形下的一种扩展形式,EKF算法是将非线性函数进行泰勒展开,省略高阶项,保留展开项的一阶项,以此来实现非线性函数线性化,最后通过卡尔曼...
  • 卡尔曼滤波

    2019-12-30 19:15:10
    关于卡尔曼滤波的简单解释 卡尔曼滤波直观解释: 假设两个传感器,测的是同一个信号。可是它们每次的读数都不太一样,怎么办? 取平均。 假设已知其中贵的传感器器应该准一些,便宜的那个应该差一些。那有比取...
  • 20180626 卡尔曼滤波算法计算SOC

    万次阅读 2018-06-28 08:48:54
    卡尔曼滤波算法包括线性卡尔曼滤波(KF),扩展卡尔曼(EKF),自适应卡尔曼(AEKF),以及无迹卡尔曼(UKF)等卡尔曼变形模式,线性卡尔曼滤波器针对线性系统,...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 407
精华内容 162
关键字:

拓展卡尔曼滤波