一、简介

1 卡尔曼滤波是什么
卡尔曼滤波适用于估计一个动态系统的最优状态。即便是观测到的系统状态参数含有噪声，观测值不准确，卡尔曼滤波也能够完成对状态真实值的最优估计。网上大多数的教程讲到卡尔曼的数学公式推导，会让人很头疼，难以把握其中的主线和思想。所以我参考了国外一位学者的文章，讲述卡尔曼滤波的工作原理，然后编写了一个基于OpenCV的小程序给大家做一下说明。

2 卡尔曼滤波能做什么
假设我们手头有一辆DIY的移动小车。这辆车的外形是这样的：【预测模型】基于卡尔曼滤波实现运动轨迹预测matlab源码_matlab
这辆车可以在荒野移动，为了便于对它进行控制，需要知道它的位置以及移动速度。所以，建立一个向量，用来存储小车的位置和速度【预测模型】基于卡尔曼滤波实现运动轨迹预测matlab源码_matlab_02
其实，一个系统的状态有很多，选择最关心的状态来建立这个状态向量是很重要的。例如，状态还有水库里面水位的高低、炼钢厂高炉内的温度、平板电脑上面指尖触碰屏幕的位置等等这些需要持续跟踪的物理量。好了，回归到正题，小车上面安装了GPS传感器，这个传感器的精度是10米。但是如果小车行驶的荒野上面有河流和悬崖的话，10米的范围就太大，很容易掉进去进而无法继续工作。所以，单纯靠GPS的定位是无法满足需求的。另外，如果有人说小车本身接收操控着发送的运动指令，根据车轮所转动过的圈数时能够知道它走了多远，但是方向未知，并且在路上小车打滑车轮空转的现象绝对是不可避免。所以，GPS以及车轮上面电机的码盘等传感器是间接地为我们提供了小车的信息，这些信息包含了很多的和不确定性。如果将所有这些信息综合起来，是否能够通过计算得到我们更想要的准确信息呢？答案是可以的！【预测模型】基于卡尔曼滤波实现运动轨迹预测matlab源码_matlab_03
3 卡尔曼滤波的工作原理
1.先验状态估计
以之前我们创建的状态变量为例，【预测模型】基于卡尔曼滤波实现运动轨迹预测matlab源码_matlab_04
下图表示的是一个状态空间图，为了研究方便，假如小车在一条绝对笔直的线路上面行驶，其位置和速度的方向是确定的，不确定的是大小。【预测模型】基于卡尔曼滤波实现运动轨迹预测matlab源码_matlab_05【预测模型】基于卡尔曼滤波实现运动轨迹预测matlab源码_matlab_06【预测模型】基于卡尔曼滤波实现运动轨迹预测matlab源码_matlab_07

二、源代码

clear;
clc;
%采样点的个数
N=228;
%测试数据：纬度
latitude=load('C:\Users\lenovo\Desktop\基于MATLAB的运动轨迹预测，卡尔曼滤波实现\latitude.txt');
%真实维度值
lat=latitude;
%卡尔曼滤波处理的状态，即估计值
lat_kf=zeros(1,N);
%测报值
lat_z=zeros(1,N);
P=zeros(1,N);
%初始纬度值
lat(1)=29.8131;
%初始值的协方差
P(1)=0.09;
%初始测报值
lat_z(1)=29.8027;
%初始估计状态。假设和初始测报值相同
lat_kf(1)=lat_z(1);
%噪声方差
%系统噪声方差
Q=0.1;
%测量噪声方差
R=0.001;
%方差决定噪声大小
W=sqrt(Q)*randn(1,N);
V=sqrt(R)*randn(1,N);
%系统矩阵
F=1;
G=1;
H=1;
%本系统状态为1维
I=eye(1);
%模拟纬度测报，并滤波
for k=2:N
    %随时间推移，飞行纬度逐渐变化
    %k时刻的真是纬度值是测报仪器不知道的，测报值可能是无限接近于真实值，但并不是真实值
    %lat(k)=F*lat(k-1)+G*W(k-1);
    %纬度在k时刻的测报值
    lat_z(k)=H*lat(k)+V(k);
    %kalman滤波
    %有了k时刻的测报值lat_z(k)和k-1时刻的状态，那么就可以进行滤波了
    %状态预测
    lat_pre=F*lat_kf(k-1);
    %协方差预测
    P_pre=F*P(k-1)*F'+Q;
    %计算卡尔曼增益
    Kg=P_pre*inv(H*P_pre*H'+R);
    %新息
    e=lat_z(k)-H*lat_pre;
    %状态更新
    lat_kf(k)=lat_pre+Kg*e;
    %协方差更新
    P(k)=(I-Kg*H)*P_pre;
end
%计算误差
%测量值与真实值之间的偏差
Err_Messure=zeros(1,N);
%kalman估计与真实值之间的偏差
Err_Kalman=zeros(1,N);
for k=1:N
    Err_Messure(k)=abs(lat_z(k)-lat(k));
    Err_Kalman(k)=abs(lat_kf(k)-lat(k));
end
t=1:N;
  • 1.
  • 2.
  • 3.
  • 4.
  • 5.
  • 6.
  • 7.
  • 8.
  • 9.
  • 10.
  • 11.
  • 12.
  • 13.
  • 14.
  • 15.
  • 16.
  • 17.
  • 18.
  • 19.
  • 20.
  • 21.
  • 22.
  • 23.
  • 24.
  • 25.
  • 26.
  • 27.
  • 28.
  • 29.
  • 30.
  • 31.
  • 32.
  • 33.
  • 34.
  • 35.
  • 36.
  • 37.
  • 38.
  • 39.
  • 40.
  • 41.
  • 42.
  • 43.
  • 44.
  • 45.
  • 46.
  • 47.
  • 48.
  • 49.
  • 50.
  • 51.
  • 52.
  • 53.
  • 54.
  • 55.
  • 56.
  • 57.
  • 58.
  • 59.
  • 60.
  • 61.
  • 62.
  • 63.
  • 64.
  • 65.
  • 66.
  • 67.

三、运行结果

【预测模型】基于卡尔曼滤波实现运动轨迹预测matlab源码_matlab_08