logo资料库

卡尔曼滤波学习笔记.docx

发布时间：2022-06-15 发布人：admin 分类：说明书 资料大小：0.13M 资料格式：docx 举报 版权申诉
第1页 / 共8页
第2页 / 共8页
第3页 / 共8页
第4页 / 共8页
第5页 / 共8页
第6页 / 共8页
第7页 / 共8页
第8页 / 共8页
资料共8页，全文预览结束
    一、前言
    二、问题描述与定义
    1、前提
    2、问题描述
    3、举例
    三、算法流程
    预测：
    更新：
    四、算法实现
    引用
    我对卡尔曼滤波的理解 一、前言 卡尔曼滤波器是一种最优线性状态估计方法（等价于“在最小均方误差准则 下的最佳线性滤波器”），所谓状态估计就是通过数学方法寻求与观测数据最佳拟 合的状态向量。 在移动机器人导航方面，卡尔曼滤波是最常用的状态估计方法。直观上来讲， 卡尔曼滤波器在这里起了数据融合的作用，只需要输入当前的测量值（多个传感 器数据）和上一个周期的估计值就能估计当前的状态，这个估计出来的当前状态 综合考量了传感器数据（即所谓的观察值、测量值）和上一状态的数据，为当前 最优估计，可以认为这个估计出来的值是最可靠的值。由于我们在 SLAM 中主要 用它做位置估计，所以前面所谓的估计值就是估计位置坐标了，而输入的传感器 数据包括码盘推算的位置、陀螺仪的角速度等（当然可以有多个陀螺仪和码盘）， 最后输出的最优估计用来作为机器人的当前位置被导航算法以外的其他程序所 调用。 列举一下卡尔曼滤波的优点：采用递归方法解决线性滤波问题，只需要当前 的测量值和前一个采样周期的估计值就能够进行状态估计，不需要大量的存储空 间，每一步的计算量小，计算步骤清晰，非常适合计算机处理。 二、问题描述与定义 1、前提 首先明确卡尔曼滤波器的前提假设： （1） 信息过程的足够精确的模型，是由白噪声所激发的线性、离散和有限 维动态系统（可以是时变的）； （2） 每次测量信号都包含着附加的白噪声分量。 满足上述条件就可以使用卡尔曼滤波器。 2、问题描述 定义一个随机离散时间过程的状态向量 x R ，该过程用一个离散随机差分 n 方程描述： x k  Ax k 1   Bu k 1   w k 1  其中 n 维向量 kx 为 k 时刻的系统状态变量，n 维向量 1kx  是 k-1 时刻的系统状态 变量。A 是状态转移矩阵或者过程增益矩阵，是 n n 阶方阵，它将 k-1 时刻状态
    和当前的 k 时刻状态联系起来。B 是可选的控制输入 u R 的增益，在大多数实 l 际情况下并没有控制增益，所以 1kBu  这一项很愉快的变成零了。 1kw  是 n 维向量， 代表过程激励噪声，它对应了 kx 中每个分量的噪声，是期望为 0，协方差为 Q 的 高斯白噪声， ~ kw N Q 。 (0, ) 再定义一个观测变量 z R ，得到观测方程： m z k  Hx k  v k 其中观测值 kz 是 m 阶向量，状态变量 kx 是 n 阶向量。H 是 m n 阶矩阵，代表状 态变量 kx 对测量变量 kz 的增益。观测噪声 kv 是期望为 0，协方差为 R 的高斯白噪 声， ~ kv N (0, ) R 。 3、举例 如果对上面两个公式不太明白，我举一个例子说明： 在目标跟踪的应用中，假设质点坐标为  ,x y 是直接观测得到的，质点在 x、y 轴 方向速度分别为 xv 、 yv ，那么系统状态变量 x k   , , x y v v , x T  y ，系统观测变量 kz   , x y T  ，系统没有控制输入，所以状态方程就成了： x k  Ax k w  1 k 它的状态转移矩阵 A 根据运动学公式确定： t  0 1 0 1 0 0 1 0 0 0 0 A        1  0   t   0  1  其实用矩阵的形式写开上面的方程就很好理解了： x k  Ax k w  1 k  1         x y v v x y        k         1 0 0 1 0 0 0 0 t  0 1 0 0 t  0 1               x y v v x y        k 1   w k 1   x y        t v    t v   v v x y x y        k 1   w k 1   这里简要说一下状态转移矩阵 A 的确定： （1） 在标量卡尔曼滤波中，比如测量值是温度、湿度，一般认为下一个时
    刻该温度或者湿度维持不变，这种情况下状态转移矩阵通常就是标量 1。 （2） 在导航和目标跟踪中卡尔曼滤波常被用来做位置估计，比如匀速直线 运动（CV）和匀加速直线运动（CA）： A CV       1 T 0 1 0 0 1 0   0 ,    A CA       T T 1 0 1 0 0 2 T 1 2      在前面的举例中 x,y 方向上的运动就被微分近似为匀速直线运动。 （3） 若被估计的过程或观测变量与过程的关系是非线性的，此时不能够直 接应用卡尔曼滤波（因为不满足上一节提到的卡尔曼滤波适用的前提 必须是线性系统），这个时候扩展卡尔曼滤波（EKF）应运而生，它用 雅克比矩阵将期望和方差线性化，从而将卡尔曼滤波扩展到非线性系 统，但是 EKF 由于考虑了泰勒级数的展开，运算量大大增加。 该过程的观测方程为 z k  Hx k  v k 它的观测矩阵 H 也是要指定的，它的目的是将 m 维的测量值转换到 n 维与状态 变量相对应，由于直接观测的量是位置  kz  两个元素就够了，所以 H 设计成如下： ，我们只需要取状态变量的前 , x y  T H     1 0 0 0 0 0 0 1    用矩阵形式写开就是 z k  Hx k   v k   x   y    k     1 0 0 0 0 0 0 1            我再举一个简单例子： x y v v x y         v k  x     y   k  v k 一个运动目标的状态变量 ,其中第一项是目标在 k 时 刻的位置，这里假设为一维坐标，第二项为 k 时刻的速度，第三项为 k 时刻 的加速度，雷达仅能观测到目标的位置即   x k ,那么它的状态转移矩阵就该  kx ,   x k  ,   x k     x k T  设计成： 它的观测矩阵就该设计成： A  1 0 0      t  1 0  2 t t  1 2     
    H   1 0 0  到了这里怎么设计矩阵 A 和 H 应该有思路了吧。 三、算法流程 接下来就开始介绍卡尔曼滤波最核心的五个更新方程了 预测： 更新： ˆ kx ˆ kP  ˆk Ax 1   Bu k 1   AP A Q  T 1  ˆ k ˆ x k  ˆ x k  ˆ K z Hx k  ( k k ˆ P H k ˆ T HP H k T  R ˆ I K H P  k ) k K k  ˆ P k  ( ) (1) (2) (3) (4) (5) 先来解释一下公式中各个变量的含义： ˆkx ：表示 k 时刻先验状态估计值，这是算法根据前次迭代结果（就是上一次循环 的后验估计值）做出的不可靠估计。 ˆkx 、 1ˆkx  ：分别表示 k 时刻、k-1 时刻后验状态估计值，也就是要输出的该时刻最 优估计值，这个值是卡尔曼滤波的结果。 A ：表示状态转移矩阵，是 n n 阶方阵，它是算法对状态变量进行预测的依据， 状态转移矩阵如果不符合目标模型有可能导致滤波发散，它的确定请参看第二节 中的举例。 B ：表示可选的控制输入 u R 的增益，在大多数实际情况下并没有控制增益。 l
    1ku  ：表示 k-1 时刻的控制增益，一般没有这个变量，可以设为 0。 ˆ kP ：表示 k 时刻的先验估计协方差，这个协方差矩阵只要确定了一开始的 0 ˆP ，后 面都可以递推出来，而且初始协方差矩阵 0 ˆP 只要不是为 0，它的取值对滤波效果 影响很小，都能很快收敛。 ˆ kP 、 1 ˆ kP  ：分别表示 k 时刻、k-1 时刻的后验估计协方差，是滤波结果之一。 Q ：表示过程激励噪声的协方差，它是状态转移矩阵与实际过程之间的误差。这 个矩阵是卡尔曼滤波中比较难确定的一个量，一般有两种思路：一是在某些稳定 的过程可以假定它是固定的矩阵，通过寻找最优的 Q 值使滤波器获得更好的性 能，这是调整滤波器参数的主要手段，Q 一般是对角阵，且对角线上的值很小， 便于快速收敛；二是在自适应卡尔曼滤波（AKF）中 Q 矩阵是随时间变化的。 kK ：表示卡尔曼增益，是滤波的中间结果。 kz ：表示测量值，是 m 阶向量。 H ：表示量测矩阵，是 m n 阶矩阵，它把 m 维测量值转换到 n 维与状态变量相 对应。 R ：表示测量噪声协方差，它是一个数值，这是和仪器相关的一个特性，作为已 知条件输入滤波器。需要注意的是这个值过大过小都会使滤波效果变差，且 R 取值越小收敛越快，所以可以通过实验手段寻找合适的 R 值再利用它进行真实的 滤波。 四、算法实现 以下是一段卡尔曼滤波与平滑滤波的对比 clear clc; N=300; CON = 25; %房间温度，假定温度是恒定的 %%%%%%%%%%%%%%%卡尔曼滤波%%%%%%%%%%%%%%%%%%%%%% x = zeros(1,N);
    y = 2^0.5 * randn(1,N) + CON; 加过程噪声的测量值 x(1) = 1; p = 10; Q = cov(randn(1,N)); %过程噪声协方差 R = cov(randn(1,N)); %观测噪声协方差 for k = 2 : N x(k) = x(k - 1); %预估计k时刻状态变量的值 p = p + Q; %对应于预估值的协方差 kg = p / (p + R);%kalman gain x(k) = x(k) + kg * (y(k) - x(k)); p = (1 - kg) * p; end %%%%%%%%%%平滑滤波%%%%%%%%%%%%%%%%%%%%%%% Filter_Wid = 10; smooth_res = zeros(1,N); for i = Filter_Wid + 1 : N tempsum = 0; for j = i - Filter_Wid : i - 1 tempsum = tempsum + y(j); end smooth_res(i) = tempsum / Filter_Wid; end % figure(1); % hist(y); t=1:N; figure(1); expValue = zeros(1,N); for i = 1: N expValue(i) = CON; end plot(t,expValue,'r',t,x,'g',t,y,'b',t,smooth_res,'k'); legend('real temperature','kalman result','measured value','smooth result'); axis([0 N 20 30]) xlabel('Sample Time'); ylabel('Room Temperature'); title('Smooth Filter VS Kalman Filter');
    [1] 王学斌, 徐建宏, 张章. 卡尔曼滤波器参数分析与应用方法研究[J]. 计算机应 用与软件, 2012, 29(6):212-215. 引用
    分享到：
    赞 收藏

    相关推荐

    资料库

    后端

    共收录2142份资料，累计10个分类，关注成员有19位，主要包括：Perl,Docker,Delphi,VB,PHP,Java,C,C#,Python,C++

    热门标签

    Perl Docker Delphi VB PHP Java C C# Python C++

    最新资料