|
|
相关文章推荐
|
文质彬彬的勺子 · Android动态加载jar/dex - ...· 4 月前 · |
|
|
年轻有为的茴香 · BAT执行结束后不关闭CMD窗口 - ...· 2 年前 · |
|
|
逃课的充电器 · 初级：如何在终端及图形界面中更新 ...· 2 年前 · |
|
|
不羁的青椒 · WPF(C#)开源控件库：Newbeecod ...· 2 年前 · |
卡尔曼滤波器是一种最优线性状态估计方法（等价于“在最小均方误差准则下的最佳线性滤波器”），所谓状态估计就是通过数学方法寻求与观测数据最佳拟合的状态向量。
在移动机器人导航方面，卡尔曼滤波是最常用的状态估计方法。直观上来讲，卡尔曼滤波器在这里起了数据融合的作用，只需要输入当前的测量值（多个传感器数据）和上一个周期的估计值就能估计当前的状态，这个估计出来的当前状态综合考量了传感器数据（即所谓的观察值、测量值）和上一状态的数据，为当前最优估计，可以认为这个估计出来的值是最可靠的值。由于我们在SLAM中主要用它做位置估计，所以前面所谓的估计值就是估计位置坐标了，而输入的传感器数据包括码盘推算的位置、陀螺仪的角速度等（当然可以有多个陀螺仪和码盘），最后输出的最优估计用来作为机器人的当前位置被导航算法以外的其他程序所调用。
列举一下卡尔曼滤波的优点：采用递归方法解决线性滤波问题，只需要当前的测量值和前一个采样周期的估计值就能够进行状态估计，不需要大量的存储空间，每一步的计算量小，计算步骤清晰，非常适合计算机处理。
二、问题描述与定义
首先明确卡尔曼滤波器的前提假设：
- 信息过程的足够精确的模型，是由白噪声所激发的线性、离散和有限维动态系统（可以是时变的）；
- 每次测量信号都包含着附加的白噪声分量。
满足上述条件就可以使用卡尔曼滤波器。
2、问题描述
定义一个随机离散时间过程的状态向量 ，该过程用一个离散随机差分方程描述：
其中n维向量 为k时刻的系统状态变量，n维向量 x k − 1 是k-1时刻的系统状态变量。A是状态转移矩阵或者过程增益矩阵，是 n × n 阶方阵，它将k-1时刻状态和当前的k时刻状态联系起来。B是可选的控制输入 u ∈ R l 的增益，在大多数实际情况下并没有控制增益，所以 B u k − 1 这一项很愉快的变成零了。 w k − 1 是n维向量，代表过程激励噪声，它对应了 中每个分量的噪声，是期望为0，协方差为Q的高斯白噪声， N ( 0 , Q )
再定义一个观测变量 ，得到观测方程： A C V = ⎡ ⎣ ⎢ 1 0 0 T 1 0 0 0 1 ⎤ ⎦ ⎥ , A C A = ⎡ ⎣ ⎢ ⎢ ⎢ 1 0 0 T 1 0 T 2 2 T 1 ⎤ ⎦ ⎥ ⎥ ⎥
在前面的举例中x,y方向上的运动就被微分近似为匀速直线运动。
（3） 若被估计的过程或观测变量与过程的关系是非线性的，此时不能够直接应用卡尔曼滤波（因为不满足上一节提到的卡尔曼滤波适用的前提必须是线性系统），这个时候扩展卡尔曼滤波（EKF）应运而生，它用雅克比矩阵将期望和方差线性化，从而将卡尔曼滤波扩展到非线性系统，但是EKF由于考虑了泰勒级数的展开，运算量大大增加。
该过程的观测方程为
它的观测矩阵H也是要指定的，它的目的是将m维的测量值转换到n维与状态变量相对应，由于直接观测的量是位置 z k = ( x ¯ , y ¯ ) T ，我们只需要取状态变量的前两个元素就够了，所以H设计成如下： x k = ( x ( k ) , x ˙ ( k ) , x ¨ ( k ) ) T ,其中第一项是目标在k时刻的位置，这里假设为一维坐标，第二项为k时刻的速度，第三项为k时刻的加速度，雷达仅能观测到目标的位置即 x ( k ) ,那么它的状态转移矩阵就该设计成：
A = ⎡ ⎣ ⎢ ⎢ ⎢ 1 0 0 Δ t 1 0 Δ t 2 2 Δ t 1 ⎤ ⎦ ⎥ ⎥ ⎥
它的观测矩阵就该设计成：
H = [ 1 0 0 ]
到了这里怎么设计矩阵A和H应该有思路了吧。
三、算法流程
接下来就开始介绍卡尔曼滤波最核心的五个更新方程了
1、预测：
：表示过程激励噪声的协方差，它是状态转移矩阵与实际过程之间的误差。这个矩阵是卡尔曼滤波中比较难确定的一个量，一般有两种思路：一是在某些稳定的过程可以假定它是固定的矩阵，通过寻找最优的Q值使滤波器获得更好的性能，这是调整滤波器参数的主要手段，Q一般是对角阵，且对角线上的值很小，便于快速收敛；二是在自适应卡尔曼滤波（AKF）中Q矩阵是随时间变化的。
：表示量测矩阵，是
<script type="math/tex" id="MathJax-Element-60">R</script>：表示测量噪声协方差，它是一个数值，这是和仪器相关的一个特性，作为已知条件输入滤波器。需要注意的是这个值过大过小都会使滤波效果变差，且R取值越小收敛越快，所以可以通过实验手段寻找合适的R值再利用它进行真实的滤波。
四、算法实现
以下是一段卡尔曼滤波与平滑滤波的对比
clear
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;%对应于预估值的协方差，由于状态变量是标量，状态转移矩阵A为数值1
kg = p / (p + R);%kalman gain
x(k) = x(k) + kg * (y(k) - x(k));
p = (1 - kg) * p;
%%%%%%%%%%%平滑滤波%%%%%%%%%%%%%%%%%%%%%%%%
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
);
smooth_res(i) = tempsum / Filter_Wid;
% figure(1);
% hist(y);
t=1:N;
figure(1);
expValue = zeros(1,N);
for i = 1: N
expValue(i) = CON;
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.
本文word文档下载：
http://download.csdn.net/download/u013453604/9359335
推荐文章