1、clear%%清空變數環境
clc
load shuju %%%航班航跡資料
Re = 6371393;
%真實軌跡
a_R = yout(:,1:3);
v_R = yout(:,4:6);
p_R = yout(:,7:9);
%加噪聲后的航跡計算結果
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_before = p_R_sample-p*6e6;
[Error_a, Error_v, Error_p, PP] = weizhi(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('黑線-濾波前的位置誤差 紅線-濾波后位置誤差')
%速度誤差比較
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('黑線-濾波前速度誤差 紅線-濾波后速度誤差')
%位置誤差
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('濾波后的速度誤差')
2、clc;
clear;
T=1;%雷達掃描周期
N=80/T;%總采樣次數
X=zeros(4,N);%目標的真實位置和速度
X(:,1)=[0,20,0,45];%目標的初始位置
Z=zeros(2,N);%位置的觀測
Z(:,1)=[X(1,1),X(3,1)];
delta_w=1e-2;%程序噪聲
Q=delta_w*diag([0.5,1,0.5,1]);%
R=100*eye(2);%觀測噪聲均值
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];
H=[1,0,0,0;0,0,1,0];
for t=2:N
X(:,t)=F*X(:,t-1)+sqrtm(Q)*randn(4,1);%目標真實軌跡
Z(:,t)=H*X(:,t)+sqrtm(R)*randn(2,1);%對目標的觀測
end
%Kalman Filter
Xkf=zeros(4,N);
Xkf(:,1)=X(:,1);%Kalman濾波狀態初始化
P0=eye(4);%協方差矩陣初始化
for i=2:N
Xn=F*Xkf(:,i-1);%預測
P1=F*P0*F'+Q;%觀測誤差協方差
K=P1*H'*inv(H*P1*H'+R);%增益
Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);%狀態更新
P0=(eye(4)-K*H)*P1;%濾波誤差協方差更新
end
%%誤差分析
for i=1:N
Err_Observation(i)=RMS(X(:,i),Z(:,i));
Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));
end
figure
hold on; box on;
plot(X(1,:),X(3,:),'-k');
plot(Z(1,:),Z(2,:),'-b');
plot(Xkf(1,:),Xkf(3,:),'-r');
legend('真實軌跡','觀測軌跡','濾波軌跡');
xlabel('橫坐標/m');
ylabel('縱坐標/m');
figure
hold on; box on;
plot(Err_Observation,'','MarkerFace','g');
plot(Err_KalmanFilter,'','MarkerFace','r');
legend('濾波前的誤差','濾波后的誤差');
xlabel('觀測時間/s');
ylabel('誤差');
這個程式是GPS和INS組合系統的，程式的跟蹤用卡爾曼濾波，這兩個matlab程式如果要加入其他的不確定性因素，比如風速、飛行員意圖、陀螺儀的隨機誤差，應該怎么寫代碼呢，有大佬知道嗎，我剛剛接觸matlab，感覺不太懂，求助
轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/54126.html
標籤：圖象工具使用
下一篇：語意分割iou