成人精品在线视频,国产成人综合在线,久久久久久高清

%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('濾波后的速度誤差')

復制代碼

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费