数据融合基于扩展卡尔曼滤波器实现三维数据融合matlab源码

Posted 2021-06-30 MatlabQQ1575304183

随着微型飞行器（MA Vs）变得越来越便宜和普遍存在，它们在复杂的城市环境中的使用将变得更加需要诸如检查，监视和交付之类的用途。与传统GNSS系统相比，这些环境的导航要求更高的定位精度。虽然MA V通常包含惯性测量单位（IMU），但其基于积分的状态估计值容易随时间漂移。我们探索了传感器融合的用法，以结合这些互补传感器。在这个项目中，我们使用不变扩展卡尔曼滤波器（InEKF）来估计MAV在充满挑战的城市环境中的位置。我们通过将估计位置与地面真实数据集进行比较来评估结果。

简介

---

　　已经历经了半个世纪的卡尔曼滤波至今仍然是研究的热点，相关的文章不断被发表。其中许多文章是关于卡尔曼滤波器的新应用，但也不乏改善和扩展滤波器算法的研究。而对算法的研究多着重于将卡尔曼滤波应用于非线性系统。

　　为什么学界要这么热衷于将卡尔曼滤波器用于非线性系统呢？因为卡尔曼滤波器从一开始就是为线性系统设计的算法，不能用于非线性系统中。但是事实上多数系统都是非线性的，所以如果卡尔曼滤波器不能用在非线性系统中的话，那么它的应用范围就非常有限了。如果真的是这样，卡尔曼滤波器可能早就寿终正寝或者过很久很久才会被人注意到。幸运的是早期的学者们对这个问题理解的非常深刻，而且也找到了解决方法，就是扩展卡尔曼滤波（EKF）。

　　事实上世界上的第一个卡尔曼滤波也是扩展卡尔曼滤波，而不是线性卡尔曼滤波器。扩展卡尔曼滤波有很久远的历史，如果说有一个非线性系统需要用到卡尔曼滤波的话，不必怀疑，先试试扩展卡尔曼滤波准没错。因为他有很久远的历史，所以可以轻松的找到许多这方面的资料。

　　不过扩展卡尔曼滤波也不是无懈可击的，它有一个很严重的短板——发散。使用扩展卡尔曼滤波的时候请务必记在心上，时刻提醒自己，这样设计滤波器其结果会发散吗？毫不夸张地说相对于线性卡尔曼滤波设计扩展卡尔曼滤波器的就是在解决发散问题。发散问题解决了剩下的都是小事。

小结：

• 扩展卡尔曼滤波器主要用于非线性系统；
• 扩展卡尔曼滤波器会发散。

线性化的卡尔曼滤波器

---

　　在讨论扩展卡尔曼滤波之前，首先要了解一下线性化卡尔曼滤波。它和线性卡尔曼滤波器在滤波器的算法方面有同样的算法结构，一样一样的。不一样的地方在于这两者的系统模型不同。线性卡尔曼滤波器的系统本身就是线性系统，而线性化卡尔曼滤波器的系统本身是非线性系统，但是机智的大神们将非线性的系统进行了线性化，于是卡尔曼滤波就可以用在非线性系统中了。对于一个卡尔曼滤波器的设计者，就不要去管你的模型到底是一开始就是线性系统还是非线性系统线性化得到的线性系统，反正只要是线性系统就好了。好了，现在你有了一个线性系统，那你还需要担心什么呢？这就是一个之前讲过的线性卡尔曼滤波器啦。

　　的确是这样的，没有很大的差别，但是请跟我一起念：线性化卡尔曼滤波器会发散。为什么会发散呢？是这样，我们在对非线性系统进行线性化的过程中，只有被线性化的那个点附近的线性化模型和真实的模型相近，远的误差就大了，那么这个时候卡尔曼滤波器的效果就不好。如果懂点线性化知识这个道理就很明显。所以线性化的这个限制要时刻考虑，这也就是为什么要把线性卡尔曼滤波器和线性化卡尔曼滤波器区分开的理由。

　　而决定一个线性化滤波器成功与否的关键就在于这个滤波器系统模型线性化得好不好。一个贴近于真实模型的线性化模型对于滤波器的良好输出非常重要。所以说掌握如何线性化一个非线性模型很重要，然而我们并不会讨论关于系统线性化的问题，因为这已经不属于卡尔曼滤波的范畴了，而且每个系统有不同的线性化方法，有点复杂。

　　总而言之，如果你已经明白了如何使用线性卡尔曼滤波器，那么使用线性化卡尔曼滤波器就没有什么需要担心的，因为算法结构一样嘛。唯一需要注意的就是线性化卡尔曼滤波器会发散，要在有效范围内使用，要不然滤波器的表现不会好。

小结：

• 线性化卡尔曼滤波器与线性卡尔曼滤波器有同样的算法结构，不同样的系统模型；
• 线性化卡尔曼滤波器会发散，原因在于函数在远于被线性化的点的时候并不接近于非线性函数；
• 线性化卡尔曼的表现取决于线性化做得好不好。

扩展卡尔曼滤波器

---

　　 在这节具体介绍卡尔曼滤波器。首先从比较熟悉的线性卡尔曼滤波器开始比较扩展卡尔曼滤波器与线性化卡尔曼滤波器的异同，从系统模型到滤波器算法，并解释这些不同。之后将提供两个具体的应用例子来加以体会。这门书的重点在于如何感性的理解和使用卡尔曼滤波器，所以对于算法的推导不会被具体描述。但是如何理解和区别这些不同，这些不同表达了什么意义将会一一解释。如下图所示，右面是我们已经熟悉的经典的线性化卡尔曼滤波的算法结构和步骤，左面则是扩展卡尔曼滤波器的算法结构和步骤，其中不同的地方已经用红笔圈出来。可以看到两个滤波器的算法结构是相同的，只有几个方程上有细微的差别。

　　非线性系统模型

　　在非线性系统中，系统模型是这样的：

xk+1 ＝ f（Xk）＋ wk

zk ＝ h（Xk） ＋ vk

　　这与线性系统的区别在于非线性系统的状态向量和其系数是不能够分离的。

 

　　比如说，在GPS定位的伪距与接收机位置的关系中：ρ［Xu］＝ ［（xu－xsi）2 + （yu －ysi）2 ＋（zu － zsi）2 ］1/2 ＋ b ＋ vi ，

　　其中［Xu］＝［xu，yu,zu,b］是系统状态向量，分别是接收机的位置和接收机与卫星的钟差，

　　　　［xsi，ysi，zsi］是卫星的位置坐标，ρ是伪距，vi是观测噪声。

　　在这个关系中状态向量与它的系数就是不可分离的，没有办法写成AXk的形式，只能是f（Xk）。h（Xk）的存在同理。

 

　　扩展卡尔曼滤波器算法

　　比较上图，可以看到差别主要在这样两个地方：

1. 第  I   步中xk+1 ＝ f（xk－1），与原来的xk+1 ＝ Axk－1；
2. 第 III 步中Xk ＝ Xk－ ＋ Kk（zk － h（xk－）），与原来的Xk ＝ Xk－ ＋ Kk（zk － Hxk－）；

　　其他的地方则完全一样。那么是不是只需要改动这两点就可以将一个线性卡尔曼滤波变成扩展卡尔曼滤波呢？不是的，有一些更重要的差别隐藏在公式中。在右图中，也就是线性卡尔曼滤波器中矩阵A和矩阵H是已知的，在而左图中虽然将第I步和第III步中的A和H替换成f和h，但是其他地方的A和H却仍然存在，可是在非线性系统中，哪有A和H呢？

　　A和H的获得就要涉及到之前所提到的决定扩展卡尔曼滤波器表现的决定因素：线性化。线性化的方法很经典：

 

　　将非线性系统中的f对x求（xk估计）处的偏导得到A，同样的求h对x求（xk－）处的偏导得到H。（向量和矩阵怎么求偏导？）

　　

　　线性化滤波器和扩展卡尔曼滤波器的共同点在于他们都需要经历一个线性化的过程，不同点在于，扩展卡尔曼滤波器是将xk估计作为线性化的参考点，线性化卡尔曼滤波器不是。（线性化滤波器是用什么作为线性化参考点？参考点是不是就是求偏导以后的带入值？）　在设计扩展卡尔曼滤波器的时候是不是知道这一点并不会有什么不同。但是如果你在犹豫我是要用扩展卡尔曼滤波器还是用线性化卡尔曼滤波器的时候，明白这一点是非常重要的。下面对线性化滤波器和卡尔曼滤波器线性化参考点的差异做简单的解释。

　　在扩展卡尔曼滤波中，我们并不用前一个时刻的先验值Xk－（卡尔曼滤波器未经过修正的预测值）作为参考点，而是用前一个时刻的估计值作为参考点做线性化。这是因为相对于先验值，前一个时刻的估计值更加贴近于真实值，将估计值作为线性化参考点可以得到一个更加贴近于实际的线性化系统模型。这种线性化方法跟适合难以提前确定线性化参考点的系统模型。而相反的。如果说线性化参考点已经确定了，那么完全不必用前一刻的估计值作为线性化参考点。比如说在对卫星的位置这样的系统模型进行线性化的时候，由于卫星的运动轨迹有一个连续的轨道，在这种情况下，就不必用前一个时刻的估计值作为线性化参考点。（而是直接用系统对下一个时刻的预测就可以了？）

　　总而言之，你只要有个概念，扩展卡尔曼滤波器是基于先验估计做系统线性化的就可以了。具体的细节在实验中就会有所体会。重要的是我们知道了A和H是根据上面两个公式得到的。

　　总结一下这一小结讲的什么。我们看到总体而言扩展卡尔曼滤波器的结构过程都和线性卡尔曼滤波器相同。但是每一步的等式都有一些细微的差别，这些差别可以分为两块：第一个是扩展卡尔曼滤波器用非线性系统系统方程f和h替换了线性系统的A和H。第二个是扩展卡尔曼滤波器中的矩阵A和矩阵H是非线性系统的雅可比行列式（什么鬼）。除了这两块剩下的都和线性卡尔曼滤波器相同。

 

小结：

• 在第I步和第III步中，用f和h代替原来的A和H；
• 剩下的A和H，分别求f和h在（xk估计）和（xk－）处的偏导得到；
• 扩展卡尔曼滤波器用前一个时刻的估计值作为参考点做线性化。

---

 总结：

　　所谓扩展卡尔曼滤波器，就是适用于非线性系统的卡尔曼滤波器。它与经典的线性卡尔曼滤波器很相似，算法步骤和结构都相同。不同在于系统模型和矩阵A和H。在扩展卡尔曼滤波器当中用非线性系统模型方程代替线性系统墨香的系统方程；将系统模型求偏导得到新的扩展卡尔曼滤波器当中的矩阵A和H，在偏导的求解过程中，也是就是线性化的过程中用前一个时刻的估计值作为参考点。通过这样的修改就得到了适用于非线性系统的扩展卡尔曼滤波器。在使用的过程中我们要时刻牢记，扩展卡尔曼滤波会发散。下一篇是实验。

 

%for testing

clc

clear

close all


pauseLen = 0;


%%Initializations

%TODO: load data here

data = load('lib/IMU_GPS_GT_data.mat');

IMUData = data.imu;

GPSData = data.gpsAGL;

gt = data.gt;


addpath([cd, filesep, 'lib'])

initialStateMean = eye(5);

initialStateCov = eye(9);

deltaT = 1 / 30; %hope this doesn't cause floating point problems


numSteps = 500000;%TODO largest timestamp in GPS file, divided by deltaT, cast to int


results = zeros(7, numSteps);

% time x y z Rx Ry Rz


% sys = system_initialization(deltaT);

Q = blkdiag(eye(3)*(0.35)^2, eye(3)*(0.015)^2, zeros(3));

%IMU noise characteristics

%Using default values from pixhawk px4 controller

%https://dev.px4.io/v1.9.0/en/advanced/parameter_reference.html

%accel: first three values, (m/s^2)^2

%gyro: next three values, (rad/s)^2


filter = filter_initialization(initialStateMean, initialStateCov, Q);


%IMU noise? do in filter initialization


IMUIdx = 1;

GPSIdx = 1;

nextIMU = IMUData(IMUIdx, :); %first IMU measurement

nextGPS = GPSData(GPSIdx, :); %first GPS measurement


%plot ground truth, raw GPS data


% plot ground truth positions

plot3(gt(:,2), gt(:,3), gt(:,4), '.g')

grid on

hold on

% plot gps positions

% plot3(GPSData(:,2), GPSData(:,3), GPSData(:,4), '.b')

axis equal

axis vis3d


counter = 0;

MAXIGPS = 2708;

MAXIIMU = 27050;

isStart = false;


for t = 1:numSteps

currT = t * deltaT;

if(currT >= nextIMU(1)) %if the next IMU measurement has happened

% disp('prediction')

filter.prediction(nextIMU(2:7));

isStart = true;

IMUIdx = IMUIdx + 1;

nextIMU = IMUData(IMUIdx, :);

% plot3(filter.mu(1, 5), filter.mu(2, 5), filter.mu(3, 5), 'or');

end

if(currT >= nextGPS(1) & isStart) %if the next GPS measurement has happened

% disp('correction')

counter = counter + 1;

filter.correction(nextGPS(2:4));

GPSIdx = GPSIdx + 1;

nextGPS = GPSData(GPSIdx, :);

plot3(nextGPS(2), nextGPS(3), nextGPS(4), '.r');

% plot3(filter.mu(1, 5), filter.mu(2, 5), filter.mu(3, 5), 'ok');

% plotPose(filter.mu(1:3, 1:3), filter.mu(1:3, 5), filter.mu(1:3,4));


end

results(1, t) = currT;

results(2:4, t) = filter.mu(1:3, 5); %just position so far

% plot3(results(2, t), results(3, t), results(4, t), 'or');

% disp(filter.mu(1:3, 1:3));

if pauseLen == inf

pause;

elseif pauseLen > 0

pause(pauseLen);

end

if IMUIdx >= MAXIIMU || GPSIdx >= MAXIGPS

break

end

end

plot3(results(2,:), results(3,:), results(4,:), '.b');

% xlim([-10 10]);

% ylim([-10 10]);

xlabel('x, m');

ylabel('y, m');

zlabel('z, m');


%% Evaluation

gps_score = evaluation(gt, GPSData)


results_eval = results.';

score = 0;

estimation_idx = 1;

count = 0;

for i = 2:length(gt)

score = score + norm(gt(i, 2:4) - results_eval(30 * (i-1), 2:4)) ^ 2;

count = count + 1;

end

count

score = sqrt(score / count)




%% Function

完整代码或者代写添加QQ1575304183