前言

导师突然让我用手机版的matlab给同学们演示一下gps定位和加速度积分的融合，啥都不会的我一脸懵逼，老师只是看着我说你行的。然后就让我自己下去了解了解。自己在csdn上找了几篇帖子看了看，下了个matlab mobile边做边试。CSDN上虽然已经有了这类的介绍，但自己还是再来写个详细点的版本。

更新：资源已贴
https://download.csdn.net/download/Maxf1y/18251352

安装matlab mobile

ios可以直接进入APP store中下载

安卓我没在应用商店中找到，直接在网上下载的

进入之后登陆自己的matlab账号

登陆账号后出现以上界面，选择流式传输至matlab，此时警告需要许可证编号升级，否则无法使用，可以通过进入官网获取一个月试用权限。

进入网址：

https://ww2.mathworks.cn/campaigns/products/trials.html

输入自己的邮箱账号（手机版登陆的matlab账号）
输入信息，申请一个月试用期，之后退出网页（不仅如此online）

再次在手机上打开matlab mobile 如果依然提示升级，点击升级，连接云端后，不再需要许可证进行升级，可以正常使用，打开传感器，可以看到传感器记录的信息。如上图所示

Matlab Drive Connector 安装

手机上的matlab mobile记录数据信息后，会发送到云端，电脑上的matlab可以通过Matlab Drive Connector在云端接收到这些信息，然后对数据进行处理。

进入网址：

https://www.mathworks.com/products/matlab-drive.html?s_tid=AO_MLConnector#matlab-drive-connector

下载Matlab Drive Connector,点击箭头所示进行安装

选择文件夹位置，matlab mobile传输的数据会储存在这个位置里面

Matlab Mobile 记录数据

点开箭头所示位置进入命令行界面
输入

clear all
m = mobiledev

进入传感器界面，打开加速度传感器和位置传感器（此时若没有输入m = mobiledev命令，则会提示创建m=mobiledev）(如下图)

打开需要记录的传感器开关，点击开始，进行数据纪律。然后就可以拿着手机沿着你想测量的路径行走。注意加速度的方向（自己把手机的各个方向朝向地面，看看xyz的值什么时候为9.8，此时该方向为正方向）

数据记录结束后，点击停止，结束记录数据

输入命令：

[lat, lon, t, spd，yaw] = poslog(m);
posdata.lat = lat; 
posdata.lon = lon;
posdata.t = t; 
posdata.spd = spd;
posdata.yaw = yaw;
save('posdata.mat',“posdata”);

记录GPS的位置信息并保存为posdata.mat

[a,ta]=accellog(m);
acc.a=a;
acc.t=ta;
save('acc.mat','acc');

记录加速度信息并保存为acc.mat
之后进入matlab 跳转到Drive connector文件夹中可以看到保存的mat文件

Matlab 数据处理

打开matlab，跳转到Drive connector文件夹中（安装时保存的位置）运行

%%gpsmap.m
load('posdata.mat');
lat = posdata.lat;
lon = posdata.lon;
spd = posdata.spd;
t = posdata.t;
t = t - t(1);

%%
wm = webmap('World Imagery');
s = geoshape(lat,lon);
wmline(s,'Color', 'red', 'Width', 3);

将路径放在地图上，得到如上图效果。

在github上找了一个卡尔曼滤波的程序,对得到的数据进行滤波：
https://github.com/goodstudentwhy/IMU_GPS_KF_Fusion
（程序并没有如介绍那样对加速度进行积分融合）
运行程序一下代码，将数据整合为github程序需要的形式

load('posdata.mat');
lat = posdata.lat;
lon = posdata.lon;
spd = posdata.spd;
t = posdata.t;
t = t - t(1);

load('acc.mat');
ta = acc.t;
ta = ta - ta(1);

data = [];
data(:,1) = t;
accdata = [];
accdata(:,1) = acc.t;
accdata(:,2) = acc.a(:,1)
accdata(:,3) = -acc.a(:,2)
accdata(:,4) = acc.a(:,3)

for i = 1:length(data(:,1))
    p_t = data(i,1);
    for j = 1:length(ta)
        a_t = ta(j);
        if abs(a_t - p_t) <0.05
             data(i,4) = accdata(j,2);
             data(i,5) = accdata(j,3);
        end
    end
end

data(:,2) = lon;
data(:,3) = lat;

vx=[];
vy=[];
yaw = posdata.yaw;
spd = posdata.spd;
for i=1:length(spd)
    vy(i,:) = spd(i)*sin(yaw(i));
    vx(i,:) = spd(i)*cos(yaw(i));
end

kfdata(:,1) = data(:,1);
kfdata(:,2) = data(:,4);
kfdata(:,3) = data(:,5);
kfdata(:,4) = data(:,5);   %未使用z方向加速度，随意赋值
kfdata(:,5) = data(:,2);
kfdata(:,6) = data(:,3);
kfdata(:,7) = vx;
kfdata(:,8) = vy;
kfdata(:,9) = zeros(length(data),1);
kfdata(:,10) = zeros(length(data),1);

运行github程序
GPS_IMU_KF.m（github下载）

X_ACC = data(:,4);%X轴加速度 
Y_ACC = data(:,5);%Y轴加速度
earth = 6378137;%地球半径
L = length(X_ACC);

lat_1 = zeros(L,1);
lon_1 = zeros(L,1);
lat = zeros(L,1);
lon = zeros(L,1);
d_x = zeros(L,1);
d_y = zeros(L,1);
d_lon = zeros(L,1);
d_lat = zeros(L,1);

%转为经纬度
for i = 1:L
    lon_1(i) = floor(data(i,2));
    lat_1(i) = floor(data(i,3));
    lon(i) = lon_1(i) + (mod(data(i,2),1)/60)*100;
    lat(i) = lat_1(i) + (mod(data(i,3),1)/60)*100;
end
%%%%%%%%%%%%%%%%%%%
%%计算距离上电距离
for i = 1:L
   d_lon(i) = lon(i)-lon(1); 
   d_lat(i) = lat(i)-lat(1);
   d_x(i) = 2 * 3.14 * earth * (d_lon(i)/360);
   d_y(i) = 2 * 3.14 * (earth / cos(d_lat(i)/360 * 2 * 3.14))*(d_lat(i)/360);
end

pm_x = d_x; %GPS经度差
am_x = X_ACC;%X轴加速度数据
pm_y = d_y;  %GPS纬度差
am_y = Y_ACC;%Y轴加速度数据
t = 0.02; %运行周期
X1kf = zeros(3,1);
X2kf = zeros(3,1);
%状态转移矩阵和协方差矩阵
F = [1,t,0.5*t*t;0,1,t;0,0,1];%状态转移矩阵
P1 = [10,0,0;0,2,0;0,0,0.8]; %定义经度纬度协方差矩阵
P2 = [10,0,0;0,2,0;0,0,0.8];
Q = [10,0,0;0,0.8,0;0,0,0.8];%过程噪声
H = [1,0,0;0,0,1];%观测矩阵
R = [100,0;0,100]; %观测噪声
I = [1,0,0;0,1,0;0,0,1];
XX = zeros(L,1);
YY = zeros(L,1);
%对经纬度分别融合
for i = 1:L
    %预测
    X1_pre = F * X1kf;
    P1_pre = F*P1*F'+Q;
    
    Z1 = [pm_x(i);5];
    e1 = Z1 - H * X1_pre;
    Kg1 = P1_pre * H' * inv(H*P1_pre*H' + R);
    %更新
    X1kf = X1_pre + Kg1*e1;
    P1 = (I - Kg1*H)*P1_pre;
    XX(i) = X1kf(1);
end
%对经纬度分别融合
for i = 1:L
    %预测
    X2_pre = F * X2kf;
    P2_pre = F*P2*F'+Q;
    Z2 = [pm_y(i);5];
    e2 = Z2 - H * X2_pre;
    Kg2 = P2_pre * H' * inv(H*P2_pre*H' + R);
    %更新
    X2kf = X2_pre + Kg2*e2;
    P2 = (I - Kg2*H)*P2_pre;
    YY(i) = X2kf(1);
end

%画图
figure
hold on; box on;
plot(d_x,d_y,'r');
plot(XX,YY,'b');
legend('原始轨迹','EKF轨迹');

结果如下

GPS与加速度积分融合

鉴于在github上的程序没有进行加速度积分融合，我又在csdn上找了一位大佬的程序
链接：IMU和GPS数据融合估计位置与速度（MATLAB实现）
效果如下：

效果有点差，直接运行没有去调过参数，不知道是因为加速度积分的问题还是因为代码参数的问题。如果有大佬了解还望指点。

结语

一个比较粗糙的笔记，帮大家熟悉一下matlab mobile的使用。