主要内容：

基于GPS+IMU的卡尔曼滤波融合定位算法仿真,其中惯导用来进行状态预测,GPS用来滤波矫正，用于GPS+IMU的卡尔曼滤波融合定位算法算法编程学习！！！

部分代码：

%基本参数初始化

step = 0.01;                        %设定步长

start_time = 0;

end_time = 50;                       %设定测量时间

tspan = [start_time:step:end_time]';

N = length(tspan);

Ar = 10;

r = [Ar*sin(tspan) Ar*cos(tspan) 0.5*tspan];     %生成实际轨迹数据

v = [Ar*cos(tspan) -Ar*sin(tspan) ones(N,1)];

acc_inertial = [-Ar*sin(tspan) -Ar*cos(tspan) zeros(N,1)];

atti = [0.1*sin(tspan) 0.1*sin(tspan) 0.1*sin(tspan)];

Datti = [0.1*cos(tspan) 0.1*cos(tspan) 0.1*cos(tspan)];

g = [0 0 -9.8]';

gyro_pure = zeros(N,3);

acc_pure = zeros(N,3);

gps_pure= r;

a = wgn(N,1,1)/5;

b = zeros(N,1);

b(1) = a(1)*step;

%生成无噪声的imu数据

for iter = 1:N

  A = attiCalculator.Datti2w(atti(iter,:));

  gyro_pure(iter,:) = Datti(iter,:)*A';

  cnb = attiCalculator.a2cnb(atti(iter,:));

  acc_pure(iter,:) = cnb*(acc_inertial(iter,:)' - g);

end

运行结果：