多元信息融合报告-扩展卡尔曼滤波

目录
一、仿真背景 (2)
二、仿真算法 (2)
三、仿真结果 (4)
四、总结 (8)
图表
Figure 1EKF的算法结构 (3)
Figure 2真实路径 (5)
Figure 3不加EKF路径 (5)
Figure 4不加EKF误差 (6)
Figure 5加EKF路径 (6)
Figure 6加EKF误差 (7)
Figure 7真实、不加EKF、加EKF路径对比 (7)
Figure 8不加EKF、加EKF误差对比 (8)
一、仿真背景
室内定位很难采用GPS定位的方法，由于信号受建筑物的影响而大大衰减造成的，同时由于多路径效应也会导致定位精度较低。

在新主楼室内测试过几种GPS模块，发现只要在室内就是不能成功定位的，但只要把GPS模块靠近空旷侧的窗，GPS信号就会恢复，但是定位精度却比较差。

可见，小范围的室内定位用GPS定位方法是不合适的。

目前室内定位有很多其他方法，例如室内无线定位技术、基于计算机视觉、光跟踪定位、基于图像分析、磁场以及信标定位等。

此外，还有基于图像分析的定位技术、信标定位、三角定位等。

与GPS定位方法比较，这些技术成熟度上还存在不足，但是这些技术有很好的应用前景，尤其是在小范围、小空间的室内定位中有很好的定位效果、精度也很高，目前也有很高的研究热度。

把这些技术和GPS方法结合起来，就可以发挥各自的优长，既可以提供较好的精度和响应速度，又可以覆盖较广的范围，实现无缝的、精确的定位，把这些不同来源的传感器信息综合起来，就需要依靠多源信息融合的方法实现。

本实验主要是对室内定位数据进行滤波，从而获取更好的定位效果。

实验数据是基于图像分析的方法获取的，以实验室天花板嵌入的灯罩作为地标，灯罩具有确定的分布以及确定的位置，可以把这些灯罩作为参考，解算出摄像头的位置，再把摄像头坐标系转换到车坐标系上，这样就实现了定位。

实验数据来自于老师提供的文件，文件数据导入到matlab中，使用matlab对数据进行处理，并绘图显示出处理效果。

二、仿真算法
本文使用扩展卡尔曼滤波EKF的方法，对从图像解析出来的数据进行处理。

扩展卡尔曼滤波的原理如下：
在非线性系统中，系统模型是这样的：
xk+1 ＝f（Xk）＋wk
zk＝h（Xk）＋vk
这与线性系统的区别在于非线性系统的状态向量和其系数是不能够分离的。

所谓扩展卡尔曼滤波器，就是适用于非线性系统的卡尔曼滤波器。

它与经典的线性卡尔曼滤波器很相似，算法步骤和结构都相同。

不同在于系统模型和矩阵A和H。

在扩展卡尔曼滤波器当中用非线性系统模型方程代替线性系统墨香的系统方程；将系统模型求偏导得到新的扩展卡尔曼滤波器当中的矩阵A和H，在偏导的求解过程中，也是就是线性化的过程中用前一个时刻的估计值作为参考点。

通过这样的修改就得到了适用于非线性系统的扩展卡尔曼滤波器。

在使用的过程中我们要时刻牢记，扩展卡尔曼滤波会发散。

扩展卡尔曼滤波的算法结构如下图，注意，将非线性系统中的f对x求（xk估计）处的偏导得到A，同样的求h对x求（xk－）处的偏导得到H。

；
Figure 1EKF的算法结构
利用扩展卡尔曼滤波，就可以对我们的数据进行融合估计了。

下面对我们的matlab程序做一些说明：
1.数据存放在location.txt文件中，数据的存放格式如下：
数据分别代表：
2.程序中的变量说明：
ru, rv：pixel magnification factors，像素放大因子
xf,yf：world frame coordinates，世界坐标
xfr, yfr：robot frame coordinates，robot坐标
u0,v0：image coordinates of the camera’s principal point，相机投影点的图像坐标
zfc：distance from the optical center of the camera to ceiling plane，相机中心到天花板平面的距离
d1,d2：coordinates of Oc in the robot frame，相机中心在robot坐标系中的坐标
b：distance between the driving wheels，两轮距
P,Q：论文中的测距法模型矩阵
W：观测模型矩阵
u,v：coordinates of the feature in the image plane，image坐标中的特征位置
3.程序中的变量定义以及融合过程完全按照文章中的步骤进行，最终将得到：
a.真实的路径
b.不加EKF计算出的路径
c.加EKF计算出的路径
d.不加EKF计算和真实值之间的误差
e.加EKF计算和真实值之间的误差
三、仿真结果
根据上述数据提取以及处理的方法与流程，得到了下面的结果：
1.真实路径
Figure 2真实路径2.不加EKF路径
Figure 3不加EKF路径3.不加EKF的误差
Figure 4不加EKF误差4.加EKF的路径
Figure 5加EKF路径
5.加EKF的误差
Figure 6加EKF误差
6.真实路径、不加EKF路径、加EKF路径的综合对照
Figure 7真实、不加EKF、加EKF路径对比
7.不加EKF和加EKF误差的综合对照
Figure 8不加EKF、加EKF误差对比
四、总结
通过对比我们可以看出，加EKF后融合的数据要比不加EKF的数据好的多，无论是从路径图上还是从误差曲线上，都有明显的效果。

可见，使用滤波的方法对传感器数据进行处理可以为我们很好地提高数据的精度，这对于我们更精确地完成一些操作是必须的。

本科阶段参加过一些比赛和小制作，也经常使用一些传感器，但总是被传感器精度不足困扰。

多源信息融合这门课给了我新的工具与思路，通过采用多个不同、或者相同的传感器数据进行融合，就可以很好地提高传感器精度，获得更好的数据。

这从我们这次仿真的图像中就可以看出来它的效果。

以后再进行一些工程或者项目的时候，会尽量使用信息融合的办法，采集不同来源的数据以提高传感器精度。

这其实是对我思维的改变。

另外，这门课让我认识到自己的数学基础特别薄弱，而且面对文献耐心不足，这也是以后需要着力提升的方面。

最后，感谢老师宽限我晚交报告~
附录：matlab代码：
clear all;
clc;
tic
data=importdata('location.txt');
ru = 902.13283;
rv = 902.50141;
u0 = 347.20436;
v0 = 284.34705;
zfc = 2.1050;
d1 = -0.0668;
d2 = 0.0536;
b = 0.328;
I=eye(3);
P=[0.003 0 0;0 0.003 0; 0 0 0.01];
Q_value=0.1*[0.05 0;0 0.05];
W=0.01*[25 0 0 0;0 25 0 0;0 0 25 0;0 0 0 25];
k0(1,1:2)=data(6,:);
k0(1,3)=data(7,1);
d_s=zeros(2,2);
route_real=zeros(2,3);
dataf=zeros(2,2,2);
datat=zeros(2,2,2);
count=1;
for i=1:size(data,1)
if mod(i,7)==3
dataf(count,:,1)=data(i,:);
end
if mod(i,7)==5
dataf(count,:,2)=data(i,:);
end
if mod(i,7)==6
route_real(count,1:2)=data(i,:);
end
if mod(i,7)==0
route_real(count,3)=data(i,1);
count=count+1;
end
if mod(i,7)==2
datat(count,:,1)=data(i,:);
end
if mod(i,7)==4
datat(count,:,2)=data(i,:);
end
if mod(i,7)==1
d_s(count,:)=data(i,:);
end
end
X=k0(1,:)';
count=2;
for i=1:size(d_s,1)
k0(count,1)=k0(count-1,1)+(d_s(i,1)+d_s(i,2))/2*cos(k0(count-1,3));
k0(count,2)=k0(count-1,2)+(d_s(i,1)+d_s(i,2))/2*sin(k0(count-1,3));
k0(count,3)=k0(count-1,3)+(d_s(i,1)-d_s(i,2))/b;
count=count+1;
end
route_fusion = zeros(size(d_s,1),2);
for i=1:size(d_s,1)
X_estimate=[X(1,1)+(d_s(i,1)+d_s(i,2))/2*cos(X(3,1));X(2,1)+(d_s(i,1) +d_s(i,2))/2*sin(X(3,1));X(3,1)+(d_s(i,1)-d_s(i,2))/b];
hx=zeros(3,3);
hx(1,1)=1;
hx(2,2)=1;
hx(3,3)=1;
hx(1,3)=-(d_s(i,1)+d_s(i,2))/2*sin(X(3,1));
hx(2,3)=(d_s(i,1)+d_s(i,2))/2*cos(X(3,1));
hu=1/b*ones(3,2);
hu(3,2)=0-hu(3,2);
hu(1,1)=0.5*cos(X(3,1));
hu(2,1)=0.5*sin(X(3,1));
hu(1,2)=0.5*cos(X(3,1));
hu(2,2)=0.5*sin(X(3,1));
Q=Q_value;
Q(1,1)=Q(1,1)*abs(d_s(i,1));
Q(2,2)=Q(2,2)*abs(d_s(i,2));
P_estimate=hx*P*hx'+hu*Q*hu';
z_estimate=[-ru/zfc*(-(dataf(i,1,1)-X_estimate(1,1))*sin(X_estimate(3 ,1))+(dataf(i,2,1)-X_estimate(2,1))*cos(X_estimate(3,1))-d2)+u0;
-rv/zfc*(-(dataf(i,1,1)-X_estimate(1,1))*cos(X_estimate(3,1))-(dataf( i,2,1)-X_estimate(2,1))*sin(X_estimate(3,1))+d1)+v0;
-ru/zfc*(-(dataf(i,1,2)-X_estimate(1,1))*sin(X_estimate(3,1))+(dataf( i,2,2)-X_estimate(2,1))*cos(X_estimate(3,1))-d2)+u0;
-rv/zfc*(-(dataf(i,1,2)-X_estimate(1,1))*cos(X_estimate(3,1))-(dataf( i,2,2)-X_estimate(2,1))*sin(X_estimate(3,1))+d1)+v0];
gx = ones(4,3);
gx(1,1) = ru/zfc*sin(X(3,1));
gx(1,2) = -ru/zfc*cos(X(3,1));
gx(1,3) =
ru/zfc*(-(dataf(i,1,1)-X(1,1))*cos(X(3,1))-(dataf(i,2,1)-X(2,1))*sin( X(3,1)));
gx(2,1) = rv/zfc*cos(X(3,1));
gx(2,2) = rv/zfc*sin(X(3,1));
gx(2,3) =
rv/zfc*((dataf(i,1,1)-X(1,1))*sin(X(3,1))-(dataf(i,2,1)-X(2,1))*cos(X (3,1)));
gx(3,1) = ru/zfc*sin(X(3,1));
gx(3,2) = -ru/zfc*cos(X(3,1));
gx(3,3) =
ru/zfc*(-(dataf(i,1,2)-X(1,1))*cos(X(3,1))-(dataf(i,2,2)-X(2,1))*sin( X(3,1)));
gx(4,1) = rv/zfc*cos(X(3,1));
gx(4,2) = rv/zfc*sin(X(3,1));
gx(4,3) =
rv/zfc*((dataf(i,1,2)-X(1,1))*sin(X(3,1))-(dataf(i,2,2)-X(2,1))*cos(X (3,1)));
gx=gx*(-1);
R = gx*P_estimate*(gx')+W;
K = P_estimate*(gx')*inv(R);
Z = zeros(4,1);
Z(1,1) = datat(i,1,1);
Z(2,1) = datat(i,2,1);
Z(3,1) = datat(i,1,2);
Z(4,1) = datat(i,2,2);
X_fusion = X_estimate+K*(Z-z_estimate);
X = X_fusion;
P = (I-K*gx)*P_estimate;
route_fusion(i,:) = X_fusion(1:2,1)';
end
figure;
plot(route_real(:,1),route_real(:,2));
hold on;
plot(k0(:,1),k0(:,2),'r');
hold on;
plot(route_fusion(:,1),route_fusion(:,2),'k');
hold off;
E_odometer=zeros(size(route_real,1),1);
E_fusion=zeros(size(route_real,1),1);
for i=1:size(route_real,1)
E_odometer(i,1)=abs(k0(i,1)-route_real(i,1))+abs(k0(i,2)-route_real(i ,2));
E_fusion(i,1)=abs(route_fusion(i,1)-route_real(i,1))+abs(route_fusion (i,2)-route_real(i,2));
end
figure;
plot(1:size(route_real,1),E_odometer(:,1),'r');
hold on;
plot(1:size(route_real,1),E_fusion(:,1),'b');
hold off;
toc。