组合导航算法
① 什么是INS/GPS组合导航系统
组合导航是指综合各种导航设备，由监视器和计算机进行控制的导航系统。INS/GPS组合导航系统是指基于GPS卫星导航系统和惯性导航系统的组合导航系统。
组合导航系统模块充分利用GNSS卫星导航系统和惯性导航系统优点，基于最优估计算——卡尔曼滤波算法融合两种导航算法，获得最优的导航结果；尤其是当卫星导航系统无法工作时，利用惯性导航系统使得导航系统继续工作，保证导航系统的正常工作，提高了系统的稳定性和可靠性。
组合导航模块SKM-4DX采用GNSS(BDS/GPS系统联合定位)/INS（惯性导航）组合导航定位技术，凭借高精度六轴惯性器件和成熟的惯性算法，无需里程计或速度信号接入，且无严格安装要求，即使在隧道、车库等弱信号环境下也能为车辆提供高精度的定位模块。
② 什么是惯性导航技术，惯性导航是如何实现的
惯性导航是以牛顿力学定律为基础，通过测量载体在惯性参考系的加速度，将它对时间进行积分，且把它变换到导航坐标中，就能够得到在导航坐标中的速度、偏航角和位置等信息。但惯性导航系统由于陀螺仪零点漂移严重，车辆震动等因素，导致无法通过直接积分加速度获得高精度的方位和速度等信息，即现有的惯性导航系统很难长时间独立工作。
惯导模块是指采用GNSS(BDS/GPS系统联合定位)/INS组合导航定位技术，凭借高精度六轴惯性器件和成熟的惯性算法，无需里程计或速度信号接入，且无严格安装要求，即使在隧道、车库等弱信号环境下也能为车辆提供高精度的定位模块。
惯导模块SKM-4DX工作原理：
在车载导航中接入基于GNSS/INS组合导航定位的高性能车载组合惯导模块SKM-4DX，充分利用惯性导航系统和卫星导航系统优点，凭借高精度六轴惯性器件和成熟的惯性算法，无需里程计或速度信号接入，获得最优的导航结果；尤其是当卫星导航系统无法工作时，利用惯性导航系统使得导航系统继续工作，保证导航系统的正常工作，提高车载导航系统的稳定性和可靠性。
③ 哪位大神有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('滤波后的观测量');