gps滤波算法

‘壹’ 卡尔曼滤波理解与实现

本文为离散卡尔曼滤波算法的一 一个简明教程，从算法思想、实现过程、理论推导和程序实现四个方面阐述和分析了卡尔曼滤波算法。

XU Ruilin完成本教程主要部分的编写，WANG Xuejun完成第3节的编写，ZHU Ximin完成2.2节的编写，WEN Shuhan完成2.3节的编写，MAO Bo完成全文整理、修订和排版。

卡尔曼滤波（Kalman Filtering）及其一系列的优化和改进算法是目前在求解运动状态推算问题上最为普遍和高效的方法。 鲁道夫·卡尔曼 （Rudolf Emil Kalman） 在NASA埃姆斯研究中心访问时，发现他的方法适用于解决阿波罗计划的轨迹预测问题。阿波罗飞船的导航电脑就是使用这种滤波器进行轨迹预测。

卡尔曼滤波尤其适用于动态系统，这种方法对于内存要求极低而运算速度快，且能够保持较好的计算精度，这使得这种方法非常适合解决实时问题和应用于嵌入式系统，也就是说，卡尔曼滤波天然的适用于解决舰艇指控系统的航迹推算问题。在接下来的内容里，我们将逐步领会卡尔曼滤波的这些绝佳特点。

不过，现在我们先从复杂的舰艇航迹推算问题中解脱出来，从一个更加熟悉和简单的问题中来理解这个滤波算法的思想、过程和算法。

假设有一辆无人车WALL-E，需要导引它从A点到达B点，共有两种手段（ 图1 ）：

显然，两种方法都有一定的误差。如果单独采用某一种方法进行定位，WALL-E在误差的影响下将无法到达B点。因此，需要将两种方法结合起来，得到一个更加精确的结果，这就是卡尔曼滤波要解决的问题。

卡尔曼滤波方法如何看待我们的问题呢？在探究这个问题之前，我们先对问题进行抽象，并用数学语言来描述我们的问题。

我们用矢量 来描述WALL-E的运动状态，这个列矢量 包括位置矢量 和速度矢量 两个分量。在WALL-E的问题上，我们现在不知道位置 和速度 的准确值，但是知道WALL-E的运动模型满足 状态方程 ，定位的方法，也即观测WALL-E运动状态的方法满足 观测方程 . 当然，我们也知道，这两种方法都存在一定的误差 ，那么我们的问题就可以转化为一个优化问题——

在这一优化问题中，目标函数是要使预测（估计）误差最小，同时约束于估计方法 和 的条件下。在卡尔曼滤波中，我们的估计原则（也就是最小化估计误差的原则）是 最小方差无偏估计 [1] ，我们将通过后面的过程分析来说明这一点。

在我们正式开始引入公式分析卡尔曼滤波问题之前，我们还必须解决一个问题------把连续的线性系统离散化，也就是将连续时域问题转化为时间序列问题。当然，目前我们只讨论线性系统的情况，关于非线性系统问题，我们有扩展卡尔曼滤波（Extended Kalman Filtering, EKF）和无迹卡尔曼滤波（Unscented Kalman Filtering, UKF）两种方法来求解。

补充内容------连续线性时变系统的离散化
设连续线性时变系统的时域状态方程为

若采样周期为 ，则从时刻 到时刻 ，有

令 ， ，则离散化的状态方程为

通过对线性系统的离散化处理，我们现在可以考虑每一个时刻WALL-E的运动状态。接下来，我们将用 来表示在 时刻运动状态的最优估计值；用 表示用 时刻对 时刻的状态预测值；用 表示对 时刻综合预测和观测两种方法的最优估计值。

在估计WALL-E位置的问题上，假定我们已经知道它是匀速直线运动，WALL-E身上还携带有一个GPS传感器可以提供它的位置信息，WALL-E在前进过程中可能会遇到一些情况，比如停止前进或是受到风的影响。

加入我们已知的是WALL-E上一个时刻的最佳估计状态，即k-1时刻的位置和速度，要求的是下一时刻即k时刻的最佳估计状态，即k时刻的位置和速度，我们可以发现有两种方法可以得到它的k时刻的状态：

一种是通过WALL-E设定程序计算得到下一秒的状态，比如现在设定是匀速直线运动，那么下一秒的速度应该是恒定不变的，而位置则是在上一秒位置的基础上加上时间乘以速度即一秒内走过的路程，但是现实生活中并不是理想的，机器人会受到摩擦力、风力等的影响，当然也可能会有顽皮的小孩挡住他前进的道路，这些因素使得WALL-E在k时的真实状态与我们计算得到的数据有所不同。

另一种是通过WALL-E所携带的GPS来确定它的位置，因为GPS是测量出的就是WALL-E的实时状态，因此它比较准确。但是GPS测量k时刻的状态有两个问题，一是GPS只能测出WALL-E的位置，而测不出它的速度；二是GPS传感器测量的时候也会有仪器的误差，只能说它是比较准确的，比较接近真实值的。

那么接下来问题来了，我们如何得到k时刻WALL-E的真实状态呢？

我们将第一种方法得到的状态值称为预测值，第二种方法得到的状态值称为测量值，对汽车的最佳估计就是将这两部分信息结合起来，尽量的去逼近k时刻的真实值。

下面再深入一些思考，怎么将这两部分结合起来？

在初始时间k-1， 是WALL-E的最佳估计值，WALL-E其实可以是估计值附近的任何位置，并且这种不确定性由该概率密度函数描述。WALL-E最有可能在这个分布的平均值附近。在下一个时间，估计的不确定性增加，用一个更大的方差表示，这是因为在时间步骤k-1和k之间，WALL-E可能收到了风力的影响，或者脚可能向前滑了一点，因此，它可能已经行进了与模型预测的距离不同的距离。

WALL-E位置的另一个信息来源来自测量，方差表示误差测量的不确定性，真正的位置同样可以是平均值附近的任何位置。

预测值和测量值，对WALL-E的最佳估计是将这两部分信息结合起来，将两个概率函数相乘得到另一个高斯函数，该估计值的方差小于先前估计值，并且该概率密度函数的平均值为我们提供了WALL-E位置的最佳估计。

以下，我们将进行e的运算推导

设：

则有实际目标变量的表达式：

数学模型中目标变量的表达式：

实际模型中测量变量的表达式：

数学模型中测量变量的表达式：

将目标变量的实际值和估计值相减：

将上述方程带入误差e的表达式，我们可得出误差e的解析解：

从推导结果中我们不难看出，估计值和实际值的误差随时间呈指数形式变化，当（F-KH）<1时，随着时间的推移，会无限趋近于零，也就是意味着估计值和实际值相吻合。这就是为什么卡尔曼滤波器可以完美预测出目标状态值的原理。

在估计WALL-E位置的问题上，我们不知道位置 和速度 的准确值，但是我们可以给出一个估计区间（ 图5.a ）。卡尔曼滤波假设所有的变量是随机的且符合高斯分布（正态分布）。每个变量有一个均值 和一个方差 （ 图5.b ）。而 图5.c 则表示速度和位置是相关的。

假如我们已知上一个状态的位置值，现在要预测下一个状态的位置值。如果我们的速度值很高，我们移动的距离会远一点。相反，如果速度慢，WALL-E不会走的很远。这种关系在跟踪系统状态时很重要，它给了我们更多的信息：一个观测值告诉我们另一个观测值可能是什么样子。这就是卡尔曼滤波的目的------从所有不确定信息中提取有价值的信息。

根据数理统计知识，我们知道这种两个观测值（随机变量）之间的关系可以通过一个协方差矩阵

描述（ 图6 ）。

我们假设系统状态的分布为 高斯分布（正态分布） ，所以在 时刻我们需要两个信息：最佳预估值 及其协方差矩阵 （如式(2)所示）。

下一步，我们需要通过 时刻的状态来预测 时刻的状态。请注意，我们不知道状态的准确值，但是我们的预测函数并不在乎，它仅仅是对 时刻所有可能值的范围进行预测转移，然后得出一个k时刻新值的范围。在这个过程中，位置 和速度 的变化为

我们可以通过一个状态转移矩阵 来描述这个转换关系

同理，我们更新协方差矩阵 为

到目前为止，我们考虑的都是匀速运动的情况，也就是系统没有对WALL-E的运动状态进行控制的情况。那么，如果系统对WALL-E进行控制，例如发出一些指令启动或者制动轮子，对这些额外的信息，我们可以通过一个向量 来描述这些信息，并将其添加到我们的预测方程里作为一个修正。假如我们通过发出的指令得到预期的加速度 ，运动状态方程就更新为

引入矩阵表示为

式中 称为控制矩阵， 称为控制向量（例如加速度 ）。当然，如果没有任何外界动力影响的系统，可以忽略这一部分。

我们增加另一个细节，假如我们的预测转换矩阵不是100%准确呢，会发生什么？如果状态只会根据系统自身特性演变，那样将不会有任何问题。如果所有外界作用力对系统的影响可以被计算得十分准确，那样也不会有任何问题。但是如果有些外力我们无法预测，例如我们在跟踪一个四轴飞行器，它会受到风力影响；或者在跟踪一个轮式机器人，轮子可能会打滑，地面上的突起会使它减速。我们无法跟踪这些因素，而这些不确定事件发生时，预测方程将会失灵。因此，我们将这些不确定性统一建模，在预测方程中增加一个不确定项。

通过这种方式，使得原始状态中的每一个点可以都会预测转换到一个范围，而不是某个确定的点（ 图7.a ）。 可以这样描述------ 中的每个点移动到一个符合方差 的高斯分布里（ 图7.b ）。换言之，我们把这些不确定因素描述为方差为 的高斯噪声，并用 表示。这样就会产生一个新的高斯分布，方差不同，但是均值相同（ 图7.c ）。

通过对 的叠加扩展，得到完整的预测转换方程为

新的预测转换方程只是引入了已知的系统控制因素。新的不确定性可以通过之前的不确定性计算得到。到这里，我们得到了一个模糊的估计范围------通过 和 描述的范围。

我们之前的工作仍然是在使用运动模型一种方法来估计系统的状态，现在，我们要把另一种方法，也就是观测（本问题中为GPS定位）考虑进来，以进一步修正对运动状态的估计（ 图8 ）。

我们用矩阵 来描述观测方法的作用，于是有

再加入观测噪声 ，观测方程为

从控制论的角度出发，我们定义新息（也即观测值与预测值的误差）为

当然我们也知道，观测本身也会存在误差，比如本问题中的GPS定位精度仅有10m. 因此，我们用矩阵 来描述这种不确定性（ 图10 及 图11.a ）。

这时，我们新息的协方差为

现在我们需要把两种方法得到的可能性融合起来（ 图11.b ）。对于任何状态，有两个可能性：1. 传感器的观测值更接近系统真实状态；2. 模型推算的估计值更接近系统真实状态。如果有两个相互独立的获取系统状态的方式，并且我们想知道两者都准确的概率值，于是我们可以通过加权来解决更相信谁的问题（ 图11.c ）。

我们现在知道，系统模型的状态预测 与对系统的状态观测 服从高斯分布，把这个问题抽象一下就是——

根据我们的一个估计准则------ 最小方差估计 ，那么这个问题可以转化为优化问题求解

求导数（差分）得

则 ，从而

当维度高于一维时，我们用矩阵来描述，有

这里的 称为 卡尔曼增益 （Kalman Gain），也就是我们在解决更信任哪种方法时的偏向程度。

如果我们从两个独立的维度估计系统状态，那么根据系统模型的预测为

通过传感器的观测为

我们结合着两种方法得到

由 可知，卡尔曼增益为

将 约去（ 中也含有 项），得

此时的卡尔曼增益实际为

我们最后再来验证一下 估计的无偏性 ——

这里我们设 时刻的真值为 ，由于

由于 （ 从初值而来的无偏传递性 ）可知 ，即卡尔曼滤波满足无偏估计准则。显然，其中要求系统噪声和观测噪声是不相关、零期望的白噪声，且是线性系统，初始时刻的状态估计是无偏的。当这些条件不能满足时，卡尔曼滤波的估计结果是有偏的。

到这里，我们已经获得了卡尔曼滤波的全部要素。我们可以把整个过程总结为3个基本假设

假设一 和 都是零均值高斯白噪声，也即 ，

假设二 与 无关，也即

假设三 系统初值 的均值和方差已知，且 与 均不相关。

以及5个基本方程 方程一 状态预测

方程二 协方差预测

方程三 卡尔曼增益

‘贰’ 哪位大神有GPS与捷联惯导组合导航的卡尔曼滤波算法的matlab仿真程序

在下面的仿真的代码中，理想的观测量不是真实数据，而是自生成的正弦波数据，在真实的应用场景中，应该是一系列的参考数据。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 卡尔曼滤波器在INS-GPS组合导航中应用仿真
% Author : lylogn
% Email : [email protected]
% Company: BUAA-Dep3
% Time : 2013.01.06
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 参考文献:
% [1]. 邓正隆. 惯导技术, 哈尔滨工业大学出版社.2006.
clear all;
%% 惯性-GPS组合导航模型参数初始化
we = 360/24/60/60*pi/180; %地球自转角速度，弧度/s
psi = 10*pi/180; %psi角度 / 弧度
Tge = 0.12;
Tgn = 0.10;
Tgz = 0.10; %这三个参数的含义详见参考文献
sigma_ge=1;
sigma_gn=1;
sigma_gz=1;
%% 连续空间系统状态方程
% X_dot(t) = A(t)*X(t) + B(t)*W(t)
A=[0 we*sin(psi) -we*cos(psi) 1 0 0 1 0 0;
-we*sin(psi) 0 0 0 1 0 0 1 0;
we*cos(psi) 0 0 0 0 1 0 0 1;
0 0 0 -1/Tge 0 0 0 0 0;
0 0 0 0 -1/Tgn 0 0 0 0;
0 0 0 0 0 -1/Tgz 0 0 0;
0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0;]; %状态转移矩阵
B=[0 0 0 sigma_ge*sqrt(2/Tge) 0 0 0 0 0;
0 0 0 0 sigma_gn*sqrt(2/Tgn) 0 0 0 0;
0 0 0 0 0 sigma_gz*sqrt(2/Tgz) 0 0 0;]';%输入控制矩阵
%% 转化为离散时间系统状态方程
% X(k+1) = F*X(k) + G*W(k)
T = 0.1;
[F,G]=c2d(A,B,T);
H=[1 0 0 0 0 0 0 0 0;
0 -sec(psi) 0 0 0 0 0 0 0;];%观测矩阵
%% 卡尔曼滤波器参数初始化
t=0:T:50-T;
length=size(t,2);
y=zeros(2,length);
Q=0.5^2*eye(3); %系统噪声协方差
R=0.25^2*eye(2); %测量噪声协方差
y(1,:)=2*sin(pi*t*0.5);
y(2,:)=2*cos(pi*t*0.5);
Z=y+sqrt(R)*randn(2,length); %生成的含有噪声的假定观测值，2维
X=zeros(9,length); %状态估计值，9维
X(:,1)=[0,0,0,0,0,0,0,0,0]'; %状态估计初始值设定
P=eye(9); %状态估计协方差
%% 卡尔曼滤波算法迭代过程
for n=2:length
X(:,n)=F*X(:,n-1);
P=F*P*F'+ G*Q*G';
Kg=P*H'/(H*P*H'+R);
X(:,n)=X(:,n)+Kg*(Z(:,n)-H*X(:,n));
P=(eye(9,9)-Kg*H)*P;
end
%% 绘图代码
figure(1)
plot(y(1,:))
hold on;
plot(y(2,:))
hold off;
title('理想的观测量');
figure(2)
plot(Z(1,:))
hold on;
plot(Z(2,:))
hold off;
title('带有噪声的观测量');
figure(3)
plot(X(1,:))
hold on;
plot(X(2,:))
hold off;
title('滤波后的观测量');

‘叁’ 卡尔曼滤波算法的发展历史如何

全球定位系统（GPS）是新一代的精密卫星导航定位系统。由于其全球性、全天候以及连续实时三维定位等特点，在军事和民用领域得到了广泛的发展。近年来，随着科学技术的发展，GPS导航和定位技术已向高精度、高动态的方向发展。但是由于GPS定位包含许多误差源，尤其是测量随机误差和卫星的几何位置误差，使定位精度受到影响。利用传统的方法很难消除。而GPS动态滤波是消除GPS定位随机误差的重要方法，即利用特定的滤波方法消除各种随机误差，从而提高GPS导航定位精度。 经典的最优滤波包括：Wiener滤波和Kalman滤波。由于Wiener滤波采用频域法，作用受到限制；而Kalman滤波采用时域状态空间法，适合于多变量系统和时变系统及非平稳随机过程，且由于其递推特点容易在计算机上实现，因此得到了广泛的应用。为此，本文对Kalman滤波方法进行了深入的研究，并取得了一些成果。 本文首先概述了GPS的组成、应用及最新动态。在此基础上介绍了GPS的导航定位原理，给出了卫星可见性算法、选星算法及定位算法。然后介绍了卡尔曼滤波的基本原理，在此基础上对动态用户的飞行轨迹进行了仿真，对“singer”模型下的8状态和11状态卡尔曼滤波算法进行了仿真分析，同时对“当前”统计模型下11状态卡尔曼滤波算法进行了仿真分析，并对滤波前后的定位精度进行了比较。在此基础上，就如何提高滤波器的动态性能作者提出了改进算法，即自适应卡尔曼滤波算法、带渐消因子的优化算法及改进的优化算法，并分别进行了仿真分析。最后作者将卡尔曼滤波算法分别应用于GPS/DR和GPS/INS组合导航定位系统中，并分别对这两种系统进行了建模和仿真分析，取得了较理想的结果。 本文的研究工作，对改进传统的滤波方法有一定的参考和应用价值，并对卡尔曼滤波方法在提高GPS动态导航定位精度方面的应用起到积极的促进作用。

‘肆’ GPS定位产生偏离，怎么破

GPS处于树木遮挡、高楼林立、高架桥以及隧道和地下停车场等弱信号环境中的时候，发送的位置信息跟实际位置是有所偏差的。您是做一个什么应用呢？车载定位导航的话，可以选择一款采用能满足日益增长的车载导航对弱信号环境的高精度定位需求，即使在隧道、车库等环境下也能为车辆提供高精度定位的定位导航模块，像SKYLAB采用GNSS/INS组合导航定位技术的车载组合导航模块SKM-4DX，充分利用惯性导航系统和卫星导航系统优点，基于最优估计算法—卡尔曼滤波算法融合两种导航算法，获得最优的导航结果；尤其是当卫星导航系统无法工作时，利用惯性导航系统使得导航系统继续工作，保证导航系统的正常工作，提高了系统的稳定性和可靠性。

‘伍’ 车载导航怎么调音效

一般的2.1、5.1音响里面的0.1这个声道就是指超重低音，通常是用来接有源低音炮或者无源低音炮的，营造更好的视听效果。低音不用调，其他适当降一点，最后调音量。

车载导航是利用车载GPS（全球定位系统）配合电子地图来进行的，它能方便且准确地告诉驾驶者去往目的地的最短或者最快路径，是驾驶员的好帮手。

位于地球上空的同步卫星最初是用于军事和航空导航。美国政府在80年代时放宽了对同步卫星的使用限制，为其后来的广泛采用，打开了一个新天地。随后而来的商用通讯卫星，更是大大的增加了通讯卫星的准确性和覆盖度。

系统功能

构成车载定位导航系统的各个部分：

（1）能够导航的电子地图。

（2）地图数据的搜索和处理。

（3）定位模块。

（4）地图配比。

（5）规定行走路径。

（6）路径引导。

（7）地图实时显示模块 。

（8）人机交互接口。

（9）无线通信。

功能模块简介

GPS导航卫星准确定位、最佳路径搜索、自动语音提示。

导航模式多种目的输入，五种以上路径检索定制模式，站点设置模式，地图定位模式。

全国地图提供全国数千城市电子地图、提供最新地图支持，导航更精确让您畅行无阻。

路径规划提供出发地和目的地的路径规划，设置经由地和回避地功能。

语音提示亲切真人语音提示，行驶中自动播报道路名称。

音乐欣赏视听享受、解除疲劳。

文件管理强大的资源管理功能。

‘陆’ 什么是INS/GPS组合导航系统

组合导航是指综合各种导航设备，由监视器和计算机进行控制的导航系统。INS/GPS组合导航系统是指基于GPS卫星导航系统和惯性导航系统的组合导航系统。

组合导航系统模块充分利用GNSS卫星导航系统和惯性导航系统优点，基于最优估计算——卡尔曼滤波算法融合两种导航算法，获得最优的导航结果；尤其是当卫星导航系统无法工作时，利用惯性导航系统使得导航系统继续工作，保证导航系统的正常工作，提高了系统的稳定性和可靠性。

组合导航模块SKM-4DX采用GNSS(BDS/GPS系统联合定位)/INS（惯性导航）组合导航定位技术，凭借高精度六轴惯性器件和成熟的惯性算法，无需里程计或速度信号接入，且无严格安装要求，即使在隧道、车库等弱信号环境下也能为车辆提供高精度的定位模块。