特色栏目： python 批处理 net编程 Javascript Php Asp Css Html5 Android seo centos

【雷达通信】基于卡尔曼滤波实现GPS和INS联合导航含Matlab源码

来源：互联网收集：自由互联发布时间：2022-06-15

1 简介 ITS(智能交通系统)是一个开放的复杂巨系统,是新兴的交叉学科研究领域。ITS是将先进的信息处理技术、数据通信技术、电子控制技术以及计算机处理技术等有效的综合运用于整个

1 简介

ITS(智能交通系统)是一个开放的复杂巨系统,是新兴的交叉学科研究领域。ITS是将先进的信息处理技术、数据通信技术、电子控制技术以及计算机处理技术等有效的综合运用于整个运输管理体系,从而建立起大范围内、全方位发挥作用的实时、准确、高效的管理运输系统。而车辆导航系统时ITS应用研究的一个主要方面,是集定位系统、地理信息系统、数据库查询系统、数字蜂窝通讯技术等于一体的综合系统。近年来,如何使GPS定位技术与INS惯性导航系统更好的结合起来已经成为车辆导航系统研究中的一个重要内容。本文首先介绍了GPS定位的基本原理。然后重点研究的内容是通过信息融合,使得在定位卫星信号接收差的场合工作更可靠。在GPS/INS组合导航系统中利用卡尔曼滤波去估计系统的各种状态,并用状态的估计值去校正系统。卡尔曼滤波算法的基本思想是：采用信号与噪声的状态空间模型,利用前一时刻的估计值和现在时刻的观测值来更新对状态变量的估计,求现在时刻的估计值。它是一种递推估计算法,通过处理一系列带有误差的实际测量数据而得到的物理参数的最佳估算。最大的特点是能够剔除随机干扰噪声,从而获取逼近真实情况的有用信息。最后将本文提出的算法与现有的算法进行比较,经过Kalman滤波处理后的轨迹比原始轨迹更平滑,精度也有效提高,在GPS中断的情况下,轨迹的连续性也得到了保证。

【雷达通信】基于卡尔曼滤波实现GPS和INS联合导航含Matlab源码_gps定位

【雷达通信】基于卡尔曼滤波实现GPS和INS联合导航含Matlab源码_数据_02

2 部分代码

%GPS/INS组合导航
%量测信号：位置
%INS输出数据由simulink计算得出
clear
clc
%得到轨迹信号
load ode500
Re = 6378245; %地球长半径
%真实轨迹
a_R = yout(:,1:3);
v_R = yout(:,4:6);
p_R = yout(:,7:9);
%加噪声后的INS计算结果
a_ins = yout(:,10:12);
v_ins = yout(:,13:15);
p_ins = yout(:,16:18);
quat = yout(:,19:22); %姿态四元数
Fn = yout(:,23:25); %地理系下的比力
%惯导相关的噪声统计数据
Q_wg = (0.04/(57*3600))^2; %陀螺马氏过程
Q_wr = (0.01/(57*3600))^2; %陀螺白噪声
Q_wa = (1e-3)^2; %加计马氏过程
Q = diag([Q_wg Q_wg Q_wg, Q_wr Q_wr Q_wr, Q_wa Q_wa Q_wa]);
Tg = 300*ones(3,1);
Ta = 1000*ones(3,1);
%得到带误差的GPS输出信号
p_gps_sample = p_R(1:10:end,:);
n = size(p_gps_sample,1);
p_error(:,1:2) = 30*randn(n,2)/Re;
p_error(:,3) = 30*randn(n,1); %位置误差
p_gps = p_gps_sample+p_error; %加入位置误差
R = diag(std(p_error).^2); %计算测量噪声方差R
%卡尔曼滤波
tao= 1; %滤波步长
a_ins_sample = a_ins(1:10:end,:);
v_ins_sample = v_ins(1:10:end,:);
p_ins_sample = p_ins(1:10:end,:);
a_R_sample = a_R(1:10:end,:);
v_R_sample = v_R(1:10:end,:);
p_R_sample = p_R(1:10:end,:);
Dp= p_ins_sample-p_gps; %INS与GPS输出的位置差值
a = a_ins_sample;
v = v_ins_sample;
p = p_ins_sample;
quat0 = quat(1:10:end,:);
Fn0 = Fn(1:10:end,:);
[Error_a, Error_v, Error_p, PP] = kalman_GPS_INS_position_sp_NFb(Dp, v, p, quat0, Fn0, Q, R, Tg, Ta, tao); %得到位置，速度误差误差估计值
a_estimate = a(1:size(Error_a,1),:)-Error_a;
v_estimate = v(1:size(Error_v,1),:)-Error_v;
p_estimate = p(1:size(Error_p,1),:)-Error_p;
n = size(p_estimate,1); %行数
%位置误差比较
figure
subplot(3,1,1)
plot((1:n),(p_R_sample(1:n,1)-p(1:n,1))*6e6,'k',(1:n),(p_R_sample(1:n,1)-p_estimate(:,1))*6e6,'r') %黑线-滤波前的误差红线-滤波后的误差
xlabel('时间，单位s')
subplot(3,1,2)
plot((1:n),(p_R_sample(1:n,2)-p(1:n,2))*6e6,'k',(1:n),(p_R_sample(1:n,2)-p_estimate(:,2))*6e6,'r') %黑线-滤波前的误差红线-滤波后的误差
ylabel('位置误差，单位m')
subplot(3,1,3)
plot((1:n),p_R_sample(1:n,3)-p(1:n,3),'k',(1:n),p_R_sample(1:n,3)-p_estimate(:,3),'r') %黑线-滤波前的误差红线-滤波后的误差
xlabel('黑线-滤波前的INS误差红线-滤波后的误差')
%速度误差比较
figure
subplot(3,1,1)
plot((1:n),v_R_sample(1:n,1)-v(1:n,1),'k',(1:n),v_R_sample(1:n,1)-v_estimate(:,1),'r') %黑线-滤波前的误差红线-滤波后的误差
xlabel('时间，单位s')
subplot(3,1,2)
plot((1:n),v_R_sample(1:n,2)-v(1:n,2),'k',(1:n),v_R_sample(1:n,2)-v_estimate(:,2),'r') %黑线-滤波前的误差红线-滤波后的误差
ylabel('速度误差，单位m/s')
subplot(3,1,3)
plot((1:n),v_R_sample(1:n,3)-v(1:n,3),'k',(1:n),v_R_sample(1:n,3)-v_estimate(:,3),'r') %黑线-滤波前的误差红线-滤波后的误差
xlabel('黑线-滤波前的INS误差红线-滤波后的误差')
%位置误差
figure
subplot(3,1,1)
xlabel('时间，单位s')
plot((1:n),(p_R_sample(1:n,1)-p_estimate(:,1))*6370000,'r') %红线-滤波后的误差
subplot(3,1,2)
plot((1:n),(p_R_sample(1:n,2)-p_estimate(:,2))*6370000,'r') %红线-滤波后的误差
ylabel('位置误差，单位m')
subplot(3,1,3)
plot((1:n),p_R_sample(1:n,3)-p_estimate(:,3),'r') %红线-滤波后的误差
xlabel('滤波后的位置误差')
%速度误差
figure
subplot(3,1,1)
plot((1:n),v_R_sample(1:n,1)-v_estimate(:,1),'r') % 红线-滤波后的误差
xlabel('时间，单位s')
subplot(3,1,2)
plot((1:n),v_R_sample(1:n,2)-v_estimate(:,2),'r') %红线-滤波后的误差
ylabel('速度误差，单位m/s')
subplot(3,1,3)
plot((1:n),v_R_sample(1:n,3)-v_estimate(:,3),'r') % 红线-滤波后的误差
xlabel('滤波后的速度误差')