用卡尔曼滤波处理工程数据的方法与思考with基于GPS与INS组合导航的滤波模型仿真

Say Something：

我猜能看到这个小文章的小伙伴估计已经为了学卡尔曼滤波费劲了头脑，查遍了资料。而且我推测这里的大多数人在之前的学习过程中总是发现那些资料里总是用一些理想的模型举例子，而且针对卡尔曼滤波算法本身的讲解也大多是在展现公式，或者推导公式，并没有在工程意义上去解释它们。so，这里我将给出在我切身将其应用于工程后，我自己对卡尔曼的理解和我在用卡尔曼滤波处理工程数据时的方法与一些思考供大家参考，如有缺误，望您指正。

PS：本人正在整理一个我自己构建的松组合导航的工程记录，其中包括我本人从硬件使用到数据转译再到matlab仿真的全过程的实践。其中包含着真实数据的组合导航卡尔曼滤波算法应用以及多维度卡尔曼滤波算法仿真，有兴趣的小伙伴可以关注一下我随后的文章。

一、卡尔曼滤波的学习笔记：

这里的几张图片是我在滤波学习工程中的一些理解和笔记供大家参考：

 

二、卡尔曼滤波的理解认识：

①、性质上是一种信息融合算法，

②、时序上是非后验的，

③、信息应用范围上是对以往的量测数据有无限记忆的。

（一）、怎么从工程角度理解这几句话呢？

1.非后验：

简而言之就是滤波数据是实时给予的，并不是给你一组数据再去做处理。我们要利用当前时刻的数据以及之前的数据用某种方式提取特征进而以特征为依据对当前时刻的数据进行优化或提高精度。

2.对以往的量测数据有无限记忆：

这一点主要体现在P矩阵上，也就是协方差矩阵，它保存了对以往数据特征的记忆。

3.信息融合算法：

  首先：滤波只是一个工具，简而言之只要有对同一个体的相关特征的两组信息我们都可以滤波，但是使用工具要讲究方法，我们要知道这些信息值不值得滤？要知道：能滤和滤波结果有意义是不一样的。就比如：你在设置滤波算法的时候一般不能“自己滤自己”，我发现许多文章中作者讲的理论知识确实很通透，但是给的历程代码就是一组一维的数据“自己滤自己”（也可以看做只是构建了一个低通滤波器）———他们首先对一组有噪声的数据设置好P，Q，R然后这时候在滤波的循环里面直接就是在用上一个时刻的量测值直接当做当前时刻的估计值然后就是一番滤....这种滤波在理论意义上确实没错（使用的两组数据是：一组是现在的状态，一组是上一时刻的状态），如果你只是想浅滤一下玩玩也就够了，但是想要做信息融合工程我们就要有所思考：自己滤自己的一组的独立数据是没有意义的，（因为你在这里滤波所参考的估计值是上一时刻的量测信息，也就是说你将上一时刻量测的某些特征作为了当前量测的期望只是对数据进行了平滑）；其次，滤波所使用的两方面信息一般是在本时刻不等价的，这是什么意思呢?就比如现在有一个人身上装了两部手机，这两部手机都带有gps定位系统，然后我们要对这个人定位，这时候有的人就会说：我们可以使用这两个手机上gps的量测信息来滤波得到更精确的位置。但其实这样的融合结果往往与单个gps的信息相比没什么变化，有些时候如果你滤波初始矩阵设置的不好，这样的融合结果会很差（比单个测量还差），这是因为这样的滤波过程没有引入状态更新，也就是说我们完全依靠仪器数据来做傻瓜式融合，而并没有对这个人定位状态进行模型的构架，甚至可以说这样的算法不是滤波算法而是最小二乘算法（如果你的工程需求是这种数据融合，看到这里就可以了，之后可以去了解一下最小二乘算法）。最后：我在我的滤波学习中发现滤波基本上都是在这样两个传感器下实现的：一个高频的，精度不是很高的，传感数据可以用以时间积分的量测元件以及一个低频的，高精度的，可以直接得到与前一个传感器经时间积分后数据类型一致的数据。说的这么抽象。。。其实最典型的就是惯导和GNSS模块：前者高频测量本体三轴加速度，姿态角速度，后者低频测量位置和速度：加速度可以经积分得到速度和位置用于与GPS的量测信息来滤波融合以得到更加具有细节更加精确的运动学信息，而角速度可以积分得到姿态角以用于对载体加速度，角度矢量信息在导航坐标系下的投影。

三、基于GPS与INS组合导航的滤波模型仿真

模型背景：

三维空间：初始坐标（15,15,15）；初始速度（5,0,1）；初始加速度（1,0.5,10）

仿真时间：5s（仿真时间过长不容易在图上看到细节，所以这里仅仿真5s）

假设的GPS：量测坐标以及速度

假设的加速度计：量测加速度

仿真功能：

以低频的GPS量测与高频的INS积分估计来进行载体运动学数据融合，以得到更加精确，贴近真值的运动轨迹；

%by_zzy
close all;clear;clc;
tic;
global T N g0
g0 =10;
T = 0.01;
dur = 5;%仿真时间
N = dur/T;
%数据申请空间%
x = zeros(1,N);
y = zeros(1,N);
z = zeros(1,N);

xv = zeros(1,N);
yv = zeros(1,N);
zv = zeros(1,N);

xa = zeros(1,N);
ya = zeros(1,N);
za = zeros(1,N);
%初值设置
x(1) = 15; 
y(1) = 15;
z(1) = 15;

xv(1) = 5;
yv(1) = 0; 
zv(1) = 1;

xa(1) = 1;
ya(1) = 0.5;
za(1) = 10;

X = [x;y;z;xv;yv;zv;xa;ya;za];        
Z_ins = [ zeros(1,N); zeros(1,N); zeros(1,N); zeros(1,N); zeros(1,N); zeros(1,N);xa;ya;za];
Z_gps = [x;y;z;xv;yv; zeros(1,N); zeros(1,N); zeros(1,N); zeros(1,N)];
A = [1 0 0 T 0 0 (0.5*T^2) 0 0;
    0 1 0 0 T 0 0 (0.5*T^2) 0;
    0 0 1 0 0 T 0 0 (0.5*T^2);
    0 0 0 1 0 0 T 0 0;
    0 0 0 0 1 0 0 T 0;
    0 0 0 0 0 1 0 0 T;
    0 0 0 0 0 0 1 0 0;
    0 0 0 0 0 0 0 1 0;
    0 0 0 0 0 0 0 0 1];
H = [1 0 0 0 0 0 0 0 0;
    0 1 0 0 0 0 0 0 0;
    0 0 1 0 0 0 0 0 0;
    0 0 0 1 0 0 0 0 0;
    0 0 0 0 1 0 0 0 0;
    0 0 0 0 0 1 0 0 0;
    0 0 0 0 0 0 1 0 0;
    0 0 0 0 0 0 0 1 0;
    0 0 0 0 0 0 0 0 1];
H_ins = [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 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 1 0 0;
         0 0 0 0 0 0 0 1 0;
         0 0 0 0 0 0 0 0 1];
H_gps = [1 0 0 0 0 0 0 0 0;
         0 1 0 0 0 0 0 0 0;
         0 0 1 0 0 0 0 0 0;
         0 0 0 1 0 0 0 0 0;
         0 0 0 0 1 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 0;  
         0 0 0 0 0 0 0 0 0];
           

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%量测噪声
p_sigma2=[5;5;5];%GPS基量测，位置误差10m
v_sigma2=[0.5;0.5;0.5];%GPS基量测，速度误差0.5m/s
a_sigma2=[(0.01*g0);(0.01*g0);(0.01*g0)];%INS基量测，加速度误差0.01g
d = zeros(9,9);
d(1,1)=p_sigma2(1);d(2,2)=p_sigma2(2);d(3,3)=p_sigma2(3);
d(4,4)=v_sigma2(1);d(5,5)=v_sigma2(2);d(6,6)=v_sigma2(3);
d(7,7)=a_sigma2(1);d(8,8)=a_sigma2(2);d(9,9)=a_sigma2(3);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%生成白噪声并填入矩阵
delta_x = (normrnd(0,p_sigma2(1)^0.5,1,N))';
delta_y = (normrnd(0,p_sigma2(2)^0.5,1,N))';
delta_z = (normrnd(0,p_sigma2(3)^0.5,1,N))';

delta_xv = (normrnd(0,v_sigma2(1)^0.5,1,N))';
delta_yv = (normrnd(0,v_sigma2(2)^0.5,1,N))';
delta_zv = (normrnd(0,v_sigma2(3)^0.5,1,N))';

delta_xa = (normrnd(0,a_sigma2(1)^0.5,1,N))';
delta_ya = (normrnd(0,a_sigma2(2)^0.5,1,N))';
delta_za = (normrnd(0,a_sigma2(3)^0.5,1,N))';

V = [delta_x;delta_y;delta_z;delta_xv;delta_yv;delta_zv;delta_xa;delta_ya;delta_za];
V_ins = [zeros(1,N);zeros(1,N);zeros(1,N);zeros(1,N);zeros(1,N);zeros(1,N);delta_xa';delta_ya';delta_za'];
V_gps = [delta_x';delta_y';delta_z';delta_xv';delta_yv';zeros(1,N);zeros(1,N);zeros(1,N);zeros(1,N)];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%滤波系统噪声设置
try_sigma2 = 50;
W = normrnd(0,try_sigma2^0.5,9,N);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%R量测误差阵
R = diag([d(1,1),d(2,2),d(3,3),d(4,4),d(5,5),d(6,6),d(7,7),d(8,8),d(9,9)]);
R_ins = diag([d(7,7),d(8,8),d(9,9)]);
R_ins2 = diag([0,0,0,0,0,0,d(7,7),d(8,8),d(9,9)]);
R_gps = diag([d(1,1),d(2,2),d(3,3),d(4,4),d(5,5)]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Q系统误差阵
Q =diag([try_sigma2,try_sigma2,try_sigma2,try_sigma2,try_sigma2,try_sigma2,try_sigma2,try_sigma2,try_sigma2]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%初始协方差矩阵
P = 10*eye(9);  


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%量测数据加噪声处理
for k = 2:N
    X(:,k) = A*X(:,k-1)+W(:,k-1);       
    Z_ins(:,k) = H_ins*X(:,k)+V_ins(:,k);         
    Z_gps(:,k) = H_gps*X(:,k)+V_gps(:,k);
end


Xk = [x;y;z;xv;yv;zv;xa;ya;za];
count = 0;

%INS高频，GPS低频：INS周期为0.01;GPS周期为0.1
for k = 2:N
    if mod(count,10) ~= 0
        Xk(:,k) = A*Xk(:,k-1);    
        Xk(7:9,k) = Z_ins(7:9,k);
        
    end
           
    if mod(count,10) == 0
        Xk(:,k) = A*Xk(:,k-1);
        Xk(7:9,k) = Z_ins(7:9,k);
        P = A*P*A'+Q;
        Kg = P*H_gps(1:5,:)'/(H_gps(1:5,:)*P*H_gps(1:5,:)'+R_gps);
        Xk(:,k) = Xk(:,k) + Kg*(Z_gps(1:5,k)-H_gps(1:5,:)*Xk(:,k));
        P = P - Kg*H_gps(1:5,:)*P;     
    end
    count=count+1;
end

t = 1:N;
%画三维轨迹
figure;
plot3(X(1,t),X(2,t),X(3,t),Xk(1,t),Xk(2,t),Xk(3,t),'linewidth',1);
legend('add-noise','kalman filtered');
grid on;
toc;