robots_estimation and learning之卡尔曼滤波

最新推荐文章于 2023-12-28 12:51:15 发布

taotao1233

最新推荐文章于 2023-12-28 12:51:15 发布

阅读量1k

点赞数 2

分类专栏： slam 文章标签： kalman b

本文链接：https://blog.csdn.net/jinshengtao/article/details/81505620

版权

slam 专栏收录该内容

7 篇文章 1 订阅

订阅专栏

本次博文为cousera的第二次作业，涉及卡尔曼滤波，分别讲述了：

为什么要有运动估计
卡尔曼滤波器如何对运动状态建模
如何用贝叶斯滤波求解运动方程，其主观物理含义是什么
如果运动方程和观测方程是非线性的怎办

一、为什么要有运动估计

我们知道slam前端视觉里程计能给出一个短时间内的轨迹，但由于不可避免的误差累积，这个轨迹在长时间内是不准确的。如下图所示：

每个黑点代表视觉里程或传感器给出的估计值，但是每个估计值都是带有噪音的，噪音用云朵形状表示。我们需要计算目标真正的状态，即目标到底是朝什么样的箭头移动的。

所以 SLAM的后端优化问题，就是一个状态估计问题，我们关心这个估计带有多大的噪声，这些噪声是如何从上一时刻传递到下一时刻的、而我们又对当前的估计有多大的自信，如何从这些带有噪声的数据中，估计整个系统的状态。

人们使用状态估计理论，把这种定位和轨迹的不确定性表达出来，然后采用滤波器或非线性优化，去估计状态的均值和不确定性（方差）。

【题外话：slam和structure from motion的区别】

我们可能没有测量运动的装置，所以也可能没有运动方程。在这个情况下，有若干种处理方式：认为确实没有运动方程，或假设相机不动，或假设相机匀速运动。这几种方式都是可行的。在没有运动方程的情况下，整个优化问题就只由许多个观测方程组成。这就非常类似于SfM（Structure from Motion）问题，相当于我们通过一组图像来恢复运动和结构。与 SfM 中不同的是， SLAM 中的图像有时间上的先后顺序，而SfM 中允许使用完全无关的图像。

接下来，这次博文就以水平运动的网球为例展开。

二、卡尔曼滤波器如何对运动状态建模

假设：

网球的状态X：位置(x,y)，速度(vx,vy)

网球的观测Z：位置(x,y)

我们用离散线性模型定义网球的运动方程：

A为状态由t时刻到t+1时刻的转移矩阵，表达前后状态之间的联系

C是观测矩阵，表达状态变量和观测变量之间的联系

But是状态方程的噪声

观测方程的噪声是由于传感器的噪声导致的，状态方程的噪声是由于我们的线性状态方程不足以描述小球的运动。由于小球是在平面内直线运动，所以线性状态模型可以凑合着用。

噪声用常数矩阵表达

三、如何用贝叶斯滤波求解运动方程，其数学含义是什么

卡尔曼滤波是一种递推的状态空间方法，因此在求解时它不是直接地去寻找状态的解析解，而是利用前一状态的估计值和当前状态的观测值共同作用估计当前的状态值，以这样的方式依次递推，求取状态的每一个估计值。如下图所示，已知zt，xt-1，求xt

现在我们只有运动方程时，相当于一个患有很严重的近视人却在一个未知的地方走路。尽管他知道自己每一步走了多远，但是随着时间增长，将对自己的位置越来越不确定——内心也就越加不安。这说明在输入数据受噪声影响时，我们对位置方差的估计将越来越大。但是，当我们戴上眼镜时，由于能够不断地观测到外部清晰的场景，使得位置估计的不确定性变小了，我们就会越来越自信。如果用椭圆或椭球直观地表达协方差阵，那么这个过程有点像是在手机地图软件中走路的感觉。

我们把这里的位姿 x 和路标 y 看成服从某种概率分布的随机变量，而不是单独的一个数。因此，我们关心的问题就变成了：当我拥有某些运动数据x和观测数据z时，如何来确定状态量 x; y 的分布？进而，如果得到了新来时刻的数据之后，那么它们的分布又将发生怎样的变化？

我们可以用贝叶斯概率模型建模，首先定义

在比较常见且合理的情况下，我们假设状态量和噪声项服从高斯分布——意味着在程序中，只需要储存它们的均值xt和协方差矩阵Pt即可。均值可看作是对变量最优值的估计，而协方差矩阵则度量了它的不确定性。因此，

那么现在问题转变为：当存在一些运动数据和观测数据时，我们如何去估计状态量的高斯分布？

接下来，我们对上面两个条件概率公式应用线性动态模型，

这里的后验概率是另一个高斯分布，我们将用MAP法求解他的均值和协方差。

[由于P(zt)不含变量xt，因此求关于xt最大后验概率时可当作常数项忽略]

为了简化表达，我们定义

对上述高斯分布方程求对数，简化后：

同理，对协方差Pt也应用arg max，求偏导，将得到

最后，我们梳理下线性卡尔曼滤波的预测和更新流程

给定初始值：状态变量x和协方差矩阵P

预测：

更新：

Cousera第二次作业，要求在x-y平面每330ms预测小球的运行位置，所有参数自己调制。

红色的点表示无迹卡尔曼滤波预测结果，本次不做要求，个人认为只作为参考

完整代码路径：

https://github.com/haopo2005/robots_estimation-and-learning/tree/master/week_two

matlab代码节选：

function [ predictx, predicty, state, param ] = kalmanFilter( t, x, y, state, param, previous_t )
%UNTITLED Summary of this function goes here
%   Four dimensional state: position_x, position_y, velocity_x, velocity_y

    %% Place parameters like covarainces, etc. here:
    % P = eye(4)
    % R = eye(2)
    Bu = [0,0,0.5,0]';
    %Bu = [0,0,0,0]';
    % Check if the first time running this function
    if previous_t<0
        state = [x, y, 0, 0]';
        param.P = 250 * eye(4);
        param.A = [[1,0,0,0]',[0,1,0,0]',[0.033,0,1,0]',[0,0.033,0,1]'];
        param.C = [[1,0]',[0,1]',[0,0]',[0,0]'];
        param.R = [[0.05,0]',[0,0.05]'];
        param.sigma_m = 0.5*eye(4);
        predictx = x;
        predicty = y;
        return;
    else
        dt = t - previous_t;
        param.A = [[1,0,0,0]',[0,1,0,0]',[dt,0,1,0]',[0,dt,0,1]'];
        z_t = [x y]';
        
        X = param.A*state + Bu;
        P = param.A*param.P*param.A' + param.sigma_m;
        
        K = P*param.C'*inv(param.C*P*param.C'+param.R);
        state = X + K*(z_t - param.C*X);
        param.P = (eye(4)-K*param.C)*P;
        
        predictx = state(1);
        predicty = state(2);
    end

    %% TODO: Add Kalman filter updates
%     % As an example, here is a Naive estimate without a Kalman filter
%     % You should replace this code
%     vx = (x - state(1)) / (t - previous_t);
%     vy = (y - state(2)) / (t - previous_t);
%     % Predict 330ms into the future
%     predictx = x + vx * 0.330;
%     predicty = y + vy * 0.330;

    
    % State is a four dimensional element
    %state = [x, y, vx, vy];
end

运行结果：

原始图片