zoukankan      html  css  js  c++  java
  • 《卡尔曼滤波》 一、简介及例子

        卡尔曼滤波在大学课程《现代控制理论》当中有涉及详细讲解。卡尔曼滤波使用条件有:1、线性系统; 2、系统中噪声(不确定性)服从高斯分布。下文中的方差、误差、偏差、协方差都指不确定性的意思。误差 = 偏差;方差 = 偏差 ²。在无人驾驶导航定位当中,需要多传感器对汽车位姿进行检测 ;在多传感器融合方面,使用卡尔曼滤波理论较多。

    1、5个公式

     2、汽车例子

     

     1 clear all;
     2 close all;
     3 clc;
     4 Z = (1:100); %观测值
     5 noise = randn(1,100);%1行100列高斯白噪声
     6 Z = Z + noise; 
     7 
     8 X = [0;0];%状态值
     9 P = [1 0; 0 1];%状态协方差矩阵
    10 F = [1 1; 0 1];%状态转移矩阵
    11 Q = [0.0001, 0; 0 0.0001];%状态转移协方差矩阵
    12 H = [1 0];%观测矩阵
    13 R = 1;%观测噪声方差
    14 
    15 figure;
    16 %hold on;
    17 speed = [];
    18 distance = [];
    19 for i =1:100
    20    %% 预测
    21    X_ = F*X;
    22    P_ = F*P*F' + Q;
    23    %% 更新
    24    K = P_*H'/(H*P_*H' + R);
    25    X = X_ + K*(Z(i) - H*X_);
    26    P = (eye(2) - K*H)*P_;
    27 
    28    speed(i) = X(2);
    29    distance(i) = X(1);
    30    %%plot(X(1), X(2));
    31 end
    32 plot(distance, speed);

    3、温度估计例子

     

           在线性高斯系统中,卡尔曼滤波器构成了该系统中的最大后验概率估计。而且,由于高斯分布经过线性变换后仍服从高斯分布,所以整个过程中我们没有进行任何的近似。可以说,卡尔曼滤波器构成了线性系统的最优无偏估计

           SLAM 中的运动方程和观测方程通常是非线性函数,尤其是视觉 SLAM 中的相机模型,需要使用相机内参模型以及李代数表示的位姿,更不可能是一个线性系统。一个高斯分布,经过非线性变换后,往往不再是高斯分布,所以在非线性系统中,我们必须取一定的近似,将一个非高斯的分布近似成一个高斯分布。我们希望把卡尔曼滤波器的结果拓展到非线性系统中来,称为扩展卡尔曼滤波器(Ex-tended Kalman Filter,EKF)。通常的做法是,在某个点附近考虑运动方程以及观测方程的一阶泰勒展开,只保留一阶项,即线性的部分,然后按照线性系统进行推导。

    EKF的局限性:

    1.    首先,滤波器方法在一定程度上假设了马尔可夫性,也就是 k 时刻的状态只与 k −1时刻相关,而与 k − 1 之前的状态和观测都无关(或者和前几个有限时间的状态相关)。这有点像是在视觉里程计中,只考虑相邻两帧关系一样。如果当前帧确实与很久之前的数据有关(例如回环),那么滤波器就会难以处理这种情况。而非线性优化方法则倾向于使用所有的历史数据。它不光考虑邻近时刻的特征点与轨迹关系,更会把考虑很久之前的状态也考虑进来,称为全体时间上的 SLAM(Full-SLAM)。在这种意义下,非线性优化方法使用了更多信息,当然也需要更多的计算。

    2.与第六章介绍的优化方法相比,EKF 滤波器仅在 k−1时刻相机位姿处做了一次线性化,然后就直接根据这次线性化结果,把后验概率给算了出来。这相当于在说,我们认为该点处的线性化近似,在后验概率处仍然是有效的。而实际上,当我们离开工作点较远的时候,一阶泰勒展开并不一定能够近似整个函数,这取决于运动模型和观测模型的非线性情况。如果它们有强烈的非线性,那线性近似就只在很小范围内成立,不能认为在很远的地方仍能用线性来近似。这就是 EKF 的非线性误差,是它的主要问题所在。在优化问题中,尽管我们也做一阶(最速下降)或二阶(G-N 或 L-M)的近似,但每迭代一次,状态估计发生改变之后,我们会重新对新的估计点做泰勒展开,而不像EKF 那样只在固定点上做一次泰勒展开。这就导致优化方法适用范围更广,则在状态变化较大时亦能适用。

    3.从程序实现上来说,EKF 需要存储状态量的均值和方差,并对它们进行维护和更新。如果把路标也放进状态的话,由于视觉 SLAM 中路标数量很大,这个存储量是相当可观的,且与状态量呈平方增长(因为要存储协方差矩阵)。因此,EKF SLAM 普遍认为不可适用于大型场景。

     参考:https://blog.csdn.net/u010720661/article/details/63253509

  • 相关阅读:
    关于数据库的索引知识
    RESTful API设计相关
    Coroutine(协程)模式与线程
    Python网络编程中的服务器架构(负载均衡、单线程、多线程和同步、异步等)
    读懂diff
    Linux学习笔记——如何使用echo指令向文件写入内容
    ubuntu中执行定时任务crontab
    网络编程之异步IO,rabbitMQ笔记
    走进docker的世界之入门篇
    xml基础
  • 原文地址:https://www.cnblogs.com/winslam/p/10210760.html
Copyright © 2011-2022 走看看