1.算法概述

经度纬度和高度来自GPS信号的中的GPGGA的数据。所以提取这三个信息主要是对GPGGA中的数据进行整理。GPGGA的数据格式如下所示：

GPGGA是GPS数据输出格式语句，意思是一帧GPS定位的主要数据，是NMEA格式中使用最广的数据之一。该语句包括17个字段。

$GPGGA 语句包括17个字段：语句标识头，世界时间，纬度，纬度半球，经度，经度半球，定位质量指示，使用卫星数量，HDOP-水平精度因子，椭球高，高度单位，大地水准面高度异常差值，高度单位，差分GPS数据期限，差分参考基站标号，校验和结束标记(用回车符<CR>和换行符<LF>)，分别用14个逗号进行分隔。

Kalman滤波是在时域上运用状态空间，递推得到的一种滤波算法，便于在计算机上实时实现，计算量和存储量小。该方法可处理多变量非平稳随机过程滤波问题，可处理时变系统滤波问题。例如飞机在飞行过程中，遇到的干扰通常是时变非平稳的噪声，此时运用卡尔曼滤波可有效去除干扰，得到较真实的状态估计数据。

卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。kalman是跟踪算法的“hello world”，在当前深度学习跟踪算法遍地的情况下，kalman跟踪效果确实差强人意。但是在实际产品落地时，特别硬件不是很强大、场景不复杂的情况下，kalman还是性价比最高的。

Kalman滤波分为2个步骤，预测(predict)和校正(correct)。预测是基于上一时刻状态估计当前时刻状态，而校正则是综合当前时刻的估计状态与观测状态，估计出最优的状态。预测与校正的过程如下：

预测：

校正：

公式1是状态预测，公式2是误差矩阵预测，公式3是kalman增益计算，公式4是状态校正，其输出即是最终的kalman滤波结果，公式5是误差矩阵更新。各变量说明如下表：

2.仿真效果预览

matlab2022a仿真

3.MATLAB部分代码预览

clc;
clear;
close all;
warning off;
 
 
DATA_TYPE = 'GPGGA';%选择需要保持的数据类型
fidin     = fopen('data.txt','r');
%读取数据
tline = func_GPS_read(fidin);
 
%数据中的GPGGA部分的数据进行保存，其余数据去除，在实际中，你也可以改变下面的参数，选择其他数据
Save_data = func_save_usefull_data(tline,DATA_TYPE);
 
 
%提取维度
%数据读取
Dimensions = func_read_Dimensions(Save_data);
%去掉数据中的0值
Dimensions(find(Dimensions == 0)) = [];
%卡尔曼滤波
Dimensions_kalman = func_kalman(Dimensions);
 
figure;
subplot(211);plot(Dimensions);title('维度');
subplot(212);plot(Dimensions_kalman);title('维度kalman');
xlabel('times');
ylabel('Dimensions');
%提取经度
%数据读取
longitude = func_read_longitude(Save_data);
%去掉数据中的0值
longitude(find(longitude == 0)) = [];
%卡尔曼滤波
longitude_kalman = func_kalman(longitude);
 
figure;
subplot(211);plot(longitude);title('经度');
subplot(212);plot(longitude_kalman);title('经度kalman');
xlabel('times');
ylabel('longitude');
 
 
 
%提取高度
%数据读取
Height = func_read_Height(Save_data);
Height(find(Height == 0)) = [];
%卡尔曼滤波
Height_kalman = func_kalman(Height);
 
 
figure;
subplot(211);plot(Height);title('高度');
subplot(212);plot(Height_kalman);title('高度kalman');
xlabel('times');
ylabel('Height');
01_047_m