GPS和INS联合导航matlab程序（带数据有用的） -

%GPS/INS无反馈位置组合卡尔曼滤波器
%%%%%%%%%%%%%%%%%%
%edit by horsejun
%%%%%%%%%%%%%%%%%%
%每秒更新一次速度位置误差
%连续状态系统方程
%dx = F*x + G*w
%z = H*x + v
%离散状态系统方程
%x(k+1) = A*x(k) + B*w(k)
%z(k+1) = C*x(k+1) + v(k+1)
function [E_attitude, E_velocity, E_position, PP] = kalman_GPS_INS_position_sp_NFb(Dp, v, p, quat, Fn, Q, R, Tg, Ta, tao)
%输入
%Dp 量测位置误差，作为滤波器输入,
%Dv 量测速度误差，作为滤波器输入，
%p ins输出位置，作为滤波器系统参数
%v ins输出速度，作为滤波器系统参数
%fn ins输出导航系下比力，作为滤波器参数
%quat ins输出四元数，作为滤波器参数
%Q 系统噪声方差
%R 测量噪声方差
%Ta 加表误差漂移相关时间
%Tg 陀螺仪误差漂移相关时间
%tao 迭代步长
%%%%%%%输入向量均为行向量%%%%%%%%%%%%%
%输出
%E_position 位置预测值
%E-velocity 速度预测值
%各参数初始化
Re = 6378245; %地球长半径
e = 1/298.257; %地球扁率
wie = 7.292e-5; %地球自转角速度
% 东北天速度
Ve0 = v(:,1);
Vn0 = v(:,2);
Vu0 = v(:,3);
% 导航位置
L0 = p(:,1);
h0 = p(:,3);
%卡尔曼滤波参数初始化
PP(1:18,1:18) = diag([1/(36*57) 1/(36*57) 1/57, 0.0001 0.0001 0.0001, 0 0 1, 0.1/(57*3600) 0.1/(57*3600) 0.1/(57*3600), 0.04/(57*3600) 0.04/(57*3600) 0.04/(57*3600), 1e-4 1e-4 1e-4].^2); %初始误差协方差阵
PP0 = PP;
X = zeros(18,1); %初始状态
E_attitude = zeros(1,3);
E_position = zeros(1,3);
E_velocity = zeros(1,3);
n = size(Dp,1);
for i=1:n-1
%参数赋值
Ve = Ve0(i);
Vn = Vn0(i);
Vu = Vu0(i);
L = L0(i);
h = h0(i);
fe = Fn(i,1);
fn = Fn(i,2);
fu = Fn(i,3);
Rm = Re*(1-2*e+3*e*sin(L)^2);
Rn = Re*(1-e*sin(L)^2);
%由四元数计算姿态阵
q = quat(i,:);
Cnb = [1-2*(q(3)^2+q(4)^2), 2*(q(2)*q(3)-q(1)*q(4)), 2*(q(2)*q(4)+q(1)*q(3));
2*(q(2)*q(3)+q(1)*q(4)), 1-2*(q(2)^2+q(4)^2), 2*(q(3)*q(4)-q(1)*q(2));
2*(q(2)*q(4)-q(1)*q(3)), 2*(q(3)*q(4)+q(1)*q(2)), 1-2*(q(2)^2+q(3)^2)];
%连续系统状态转换阵 F 的时间更新
F = zeros(18,18);
F(1,2) = wie*sin(L)+Ve*tan(L)/(Rn+h);
F(1,3) = -(wie*cos(L)+Ve/(Rn+h));
F(1,5) = -1/(Rm+h);
F(1,9) = Vn/(Rm+h)^2;
F(2,1) = -(wie*sin(L)+Ve*tan(L)/(Rn+h));
F(2,3) = -Vn/(Rm+h);
F(2,4) = 1/(Rn+h);
F(2,7) = -wie*sin(L);
F(2,9) = -Ve/(Rn+h)^2;
F(3,1) = wie*cos(L)+Ve/(Rn+h);
F(3,2) = Vn/(Rm+h);
F(3,4) = tan(L)/(Rn+h);
F(3,7) = wie*cos(L)+Ve*(sec(L)^2)/(Rn+h);
F(3,9) = -Ve*tan(L)/(Rn+h)^2;
F(4,2) = -fu;
F(4,3) = fn;
F(4,4) = Vn*tan(L)/(Rm+h)-Vu/(Rm+h);
F(4,5) = 2*wie*sin(L)+Ve*tan(L)/(Rn+h);
F(4,6) = -(2*wie*cos(L)+Ve/(Rn+h));
F(4,7) = 2*wie*cos(L)*Vn+Ve*Vn*sec(L)^2/(Rn+h)+2*wie*sin(L)*Vu;
F(4,9) = (Ve*Vu-Ve*Vn*tan(L))/(Rn+h)^2;
F(5,1) = fu;
F(5,3) = -fe;
F(5,4) = -2*(wie*sin(L)+Ve*tan(L)/(Rn+h));
F(5,5) = -Vu/(Rm+h);
F(5,6) = -Vn/(Rm+h);
F(5,7) = -(2*wie*cos(L)+Ve*(sec(L)^2)/(Rn+h))*Ve;
F(5,9) = (Ve^2*tan(L)+Vn*Vu)/(Rn+h)^2;
F(6,1) = -fn;
F(6,2) = fe;
F(6,4) = 2*(wie*cos(L)+Ve/(Rn+h));
F(6,5) = 2*Vn/(Rm+h);
F(6,7) = -2*Ve*wie*sin(L);
F(6,9) = -(Vn^2+Ve^2)/(Rn+h)^2;
F(7,5) = 1/(Rm+h);
F(7,9) = -Vn/(Rm+h)^2;
F(8,4) = 1/((Rn+h)*cos(L));
F(8,7) = Ve*tan(L)/((Rn+h)*cos(L));
F(8,9) = -Ve/(cos(L)*(Rn+h)^2);
F(9,6) = 1;
F(1:3,10:12) = Cnb;
F(1:3,13:15) = Cnb;
F(4:6,16:18) = Cnb;
F(13,13) = -1/Tg(1);
F(14,14) = -1/Tg(2);
F(15,15) = -1/Tg(3);
F(16,16) = -1/Ta(1);
F(17,17) = -1/Ta(2);
F(18,18) = -1/Ta(3);
%连续系统输入矩阵更新
G = zeros(18,9);
G(1:3,1:3) = Cnb;
G(13:15,4:6) = eye(3,3);
G(16:18,7:9) = eye(3,3);
%连续系统量测阵更新
H = zeros(3,18);
H(1,7) = 1;
H(2,8) = 1;
H(3,9) = 1;
%连续系统离散化
A = eye(18,18)+F*tao;
B = (eye(18,18)+tao*F/2)*G*tao;
%卡尔曼滤波
P = A*(PP0)*A'+B*Q*B';
K = P*H'*inv(H*P*H'+R);
PP0 = (eye(18,18)-K*H)*P;
PP0 = (PP0+PP0')/2;
PP(i,:) = diag(PP0);
z = Dp(i+1,:)';
XX = A*X+K*(z-H*A*X);
X = XX;
E_attitude(i+1,:) = XX(1:3)';
E_velocity(i+1,:) = XX(4:6)';
E_position(i+1,:) = XX(7:9)';
end

复制代码

%GPS/INS组合导航
%%%%%%%%%%%%%%%%%%
%edit by horsejun
%%%%%%%%%%%%%%%%%%
%量测信号：位置
%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('滤波后的速度误差')

复制代码