基於matlab程式設計二維空間內目標作勻速直線運動和勻速圓周運動的特點原始碼
T=2;
alpha=0.8; % 加權衰減因子
window=1/(1-alpha); % 檢測機動的有效視窗長度
dt=100; % dt=dt_x=dt_y=100
Th=25; % 機動檢測門限
Ta=9.49; % 退出機動的檢測門限
N=800/T; % 取樣次數
M=50; % 模擬次數
% 真實軌跡資料
t=2:2:400;
xo0=2000+0*t;
yo0=10000-15*t;
t=402:2:600;
xo1=2000+0.075*((t-400).^2)/2;
yo1=10000-15*400-(15*(t-400)-0.075*((t-400).^2)/2);
t=602:2:610 ;
xo2=xo1(100)+15*(t-600);
yo2=yo1(100)+0*t;
t=612:2:660;
xo3=xo2(5)+(15*(t-610)-0.3*((t-610).^2)/2);
yo3=yo2(5)-0.3*((t-610).^2)/2;
t=662:2:800;
xo4=xo3(25)+0*t;
yo4=yo3(25)-15*(t-660);
x=[xo0,xo1,xo2,xo3,xo4];
y=[yo0,yo1,yo2,yo3,yo4];
e_x1=zeros(1,N);
e_x2=zeros(1,N);
e_y1=zeros(1,N);
e_y2=zeros(1,N);
px=zeros(1,N);
qy=zeros(1,N);
u=zeros(1,N);
u_a=zeros(1,N);
for j=1:M
no1=100*randn(1,N); % 隨機白噪
no2=100*randn(1,N);
for i=1:N;
zx(i)=x(i)+no1(i); % 觀測資料
zy(i)=y(i)+no2(i);
z(:,i)=[zx(i);zy(i)];
end
%
X_estimate(2,:)=[zx(2),(zx(2)-zx(1))/T,zy(2),(zy(2)-zy(1))/T];
X_est=X_estimate(2,:);
P_estimate=[dt^2,dt^2/T,0,0;dt^2/T,(dt^2)*2/(T^2),0,0;0,0,dt^2,dt^2/T;0,0,dt^2/T,(dt^2)*2/(T^2)];
x1(1)=zx(1); y1(1)=zy(1); x1(2)=X_estimate(2,1); y1(2)=X_estimate(2,3);
u(1)=0; u(2)=0;
u1=u(2);
pp=0;% ０表示非機動，１表示機動
qq=0;
rr=1;
k=3;
while k<=N
if k<=20
z1=z(:,k);
[X_pre,X_est,P_estimate,u1]=kalmanstatic(X_est,P_estimate,z1,k,u1);
X_estimate(k,:)=X_est;
X_predict(k,:)=X_pre;
P(k,:)=[P_estimate(1,1),P_estimate(1,2),P_estimate(2,2),P_estimate(3,3),P_estimate(3,4),P_estimate(4,4)];
x1(k)=X_estimate(k,1);
y1(k)=X_estimate(k,3);
u(k)=u1;
k=k+1;
else
if pp==0 % 進入非機動模型
if rr==window+1 % 由機動進入非機動模型，為防止回朔，至少遞推window＋1次
u1=0;
else
end
while rr>0
z1=z(:,k);
[X_pre,X_est,P_estimate,u1]=kalmanstatic(X_est,P_estimate,z1,k,u1);
X_estimate(k,:)=X_est;
X_predict(k,:)=X_pre;
P(k,:)=[P_estimate(1,1),P_estimate(1,2),P_estimate(2,2),P_estimate(3,3),P_estimate(3,4),P_estimate(4,4)];
x1(k)=X_estimate(k,1);
y1(k)=X_estimate(k,3);
u(k)=u1;
rr=rr-1;
end
rr=1;
if u(k)>=Th
pp=1;qq=1; % “pp=1，qq=1”表示由非機動進入機動模型
else
end
k=k+1;
else % 機動模型
if qq==1 %由非機動進入機動模型，需要進行修正
k=k-window-1;
Xm_est=[X_estimate(k-1,:),0,0];
Xm_pre=[X_predict(k,:),0,0];
Pm_estimate=zeros(6,6);
P=P(k-1,:);
m=0;
else %在機動模型中進行遞推
Xm_est=Xm_estimate(k-1,:);
end
z1=z(:,k);
[Xm_est,Pm_estimate,ua1,qq,m]=kalmandynamic(Xm_pre,Xm_est,Pm_estimate,z1,k,P,qq,m);
Xm_estimate(k,:)=Xm_est;
x1(k)=Xm_estimate(k,1);
y1(k)=Xm_estimate(k,3);
ua(k)=ua1;
if ua1<Ta % 進入非機動模型，降維，標誌 pp＝０
X_est=Xm_estimate(k,1:4);
P_estimate=Pm_estimate(1:4,1:4);
pp=0;
rr=window+1;
else
end
k=k+1;
end
end
end
for j=1:N
px(1,j)=x1(1,j)+px(1,j); % 迭加每次估計的資料
qy(1,j)=y1(1,j)+qy(1,j);
e_x1(j)=(x1(j)-x(j))+e_x1(j);
e_y1(j)=(y1(j)-y(j))+e_y1(j);
e_x2(j)=((x1(j)-x(j))^2)+e_x2(j);
e_y2(j)=((y1(j)-y(j))^2)+e_y2(j);
end
end
for k=1:N
px(1,k)=px(1,k)/M;
qy(1,k)=qy(1,k)/M;
e_x(k)=e_x1(k)/M;
ex(k)=sqrt(e_x2(k)/M-e_x(k)^2);
e_y(k)=e_y1(k)/M;
ey(k)=sqrt(e_y2(k)/M-e_y(k)^2);
end
figure(1);
plot(x,y);axis([1500 5000 -2000 12000]);
figure(2);
plot(x,y,'b-',zx,zy,'k:',px,qy,'r');
legend('真實軌跡','觀測軌跡','50次濾波軌跡');
figure(3);
plot(x,y,'k',x1,y1,'r');
legend('真實軌跡','一次濾波軌跡');
figure(4);
subplot(2,2,1),plot(e_x); title('X座標 濾波誤差均值曲線');
subplot(2,2,2),plot(e_y); title('Y座標 濾波誤差均值曲線');
subplot(2,2,3),plot(ex); title('X座標 濾波誤差標準差曲線');
subplot(2,2,4),plot(ey); title('Y座標 濾波誤差標準差曲線');
%%%%函式1靜態模型
function[X_pre,X_est,P_estimate,u1]=kalmanstatic(X_est,P_estimate,z1,k,u1)
T=2;
alpha=0.8; % 加權衰減因子
Phi=[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];
I=[1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1];
R=[10000,0;0,10000]; % 觀測噪聲方差陣
X_estimate(k-1,:)=X_est;
u(k-1)=u1;
X_predict(k,:)=(Phi*X_estimate(k-1,:)')';
P_predict=Phi*P_estimate*(Phi)';
K=P_predict*(H)'*inv(H*P_predict*(H)'+R);
X_estimate(k,:)=(X_predict(k,:)'+K*(z1-H*X_predict(k,:)'))';
P_estimate=(I-K*H)*P_predict;
X_est=X_estimate(k,:);
X_pre=X_predict(k,:);
v(:,k)=z1-H*(X_predict(k,:))'; % 新資訊
S=H*P_predict*H'+R; % 新資訊的方差陣
delta(k)=v(:,k)'*inv(S)*v(:,k);
u(k)=alpha*u(k-1)+delta(k);
u1=u(k);
%%函式2 動態模型
function [Xm_est,Pm_estimate,ua1,qq,m]=kalmandynamic(Xm_pre,Xm_est,Pm_estimate,z1,k,P,qq,m);
T=2;
I=diag([1,1,1,1,1,1]);
Phi=[1,T,0,0,(T^2)/2,0;0,1,0,0,T,0;0,0,1,T,0,(T^2)/2;0,0,0,1,0,T;0,0,0,0,1,0;0,0,0,0,0,1];
H=[1,0,0,0,0,0;0,0,1,0,0,0];
G=[(T^2)/2,0;T,0;0,(T^2)/2;0,T;1,0;0,1];
R=[10000,0;0,10000]; % 觀測噪聲方差陣
alpha=0.8; % 加權衰減因子
window=1/(1-alpha); % 檢測機動的有效視窗長度
Xm_estimate(k-1,:)=Xm_est;
if qq==1 %由非機動進入機動模型，需進行修正， 初始化
Xm_predict(k,:)=Xm_pre;
Xm_estimate(k,5)=[z1(1)-Xm_predict(k,1)]*2/(T^2);
Xm_estimate(k,6)=[z1(2)-Xm_predict(k,3)]*2/(T^2);
Xm_estimate(k,1)=z1(1);
Xm_estimate(k,3)=z1(2);
Xm_estimate(k,2)=Xm_estimate(k-1,2)+Xm_estimate(k,5)*T;
Xm_estimate(k,4)=Xm_estimate(k-1,4)+Xm_estimate(k,6)*T;
% 修正協方差陣
Pm_estimate(1,1)=R(1,1);
Pm_estimate(3,3)=R(2,2);
Pm_estimate(1,2)=R(1,1)*2/T;
Pm_estimate(2,1)=Pm_estimate(1,2);
Pm_estimate(3,4)=R(2,2)*2/T;
Pm_estimate(4,3)=Pm_estimate(3,4);
Pm_estimate(1,5)=R(1,1)*2/(T^2);
Pm_estimate(5,1)=Pm_estimate(1,5);
Pm_estimate(3,6)=R(2,2)*2/(T^2);
Pm_estimate(6,3)=Pm_estimate(3,6);
Pm_estimate(5,5)=[R(1,1)+P(1)+P(2)*2*T+P(3)*T*T]*4/(T^4);
Pm_estimate(6,6)=[R(2,2)+P(4)+P(5)*2*T+P(6)*T*T]*4/(T^4);
Pm_estimate(2,2)=R(1,1)*4/(T^2)+P(1)*4/(T^2)+P(3)+P(2)*4/T;
Pm_estimate(4,4)=R(2,2)*4/(T^2)+P(4)*4/(T^2)+P(6)+P(5)*4/T;
Pm_estimate(2,5)=R(1,1)*4/(T^3)+P(1)*4/(T^3)+P(3)*2/T+P(2)*6/(T^2);
Pm_estimate(5,2)=Pm_estimate(2,5);
Pm_estimate(4,6)=R(2,2)*4/(T^3)+P(4)*4/(T^3)+P(6)*2/T+P(5)*6/(T^2);
Pm_estimate(6,4)=Pm_estimate(4,6);
Xm_est=Xm_estimate(k,:);
qq=0;%修正後，標誌qq復位（不再初始化），ua1設為１０，使不進入非機動模型
ua1=10;
m=0;
else
% 濾波方程
Xm_predict(k,:)=(Phi*Xm_estimate(k-1,:)')';
Q=[(Xm_estimate(k-1,5)/20)^2,0;0,(Xm_estimate(k-1,6)/20)^2];
Pm_predict=Phi*Pm_estimate*(Phi)'+G*Q*G';
K=Pm_predict*(H)'*inv(H*Pm_predict*(H)'+R);
Xm_estimate(k,:)=(Xm_predict(k,:)'+K*(z1-H*Xm_predict(k,:)'))';
Pm_estimate=(I-K*H)*Pm_predict;
Xm_est=Xm_estimate(k,:);
m=m+1;
delta(k)=[Xm_estimate(k,5),Xm_estimate(k,6)]*[Pm_estimate(5,5),0;0,Pm_estimate(6,6)]*[Xm_estimate(k,5);Xm_estimate(k,6)];
if m>=window
ua(k)=delta(k)+delta(k-1)+delta(k-2)+delta(k-3)+delta(k-4);
ua1=ua(k);
else
ua1=10;
end
end
假定有一二座標雷達對一平面上運動的目標進行觀測，目標在0-400秒沿著y軸作恆速直線運動，運動速度為-15米/秒，目標的起始點為(2000米,10000米)，在t= 400-600秒向軸x方向做的慢轉彎，加速度為0.075米/秒，完成慢轉彎後加速度將降為零，從t=610秒開始做90度的快轉彎，加速度為0.3米/秒，在660秒結束轉彎，加速度降至零。雷達掃描週期T=2秒，X和Y獨立地進行觀測，觀測噪聲的標準差均為100米。描述如下：
其中，程式演算法中各引數為:
加權衰減因子, 機動檢測門限; 退出機動的檢測門限;
在跟蹤的開始,首先採用非機動模型,從第20次取樣開始,啟用機動檢測器。
通過上圖，可以看到：VD演算法有4次機動,分別對應目標的2次加速運動,和2次勻速運動,符合目標真實軌跡變化。只是在模型出現機動的時候,會出現大的誤差。在模型的調整過程中,可以明顯發現:機動檢測門限,退出機動的檢測門限,加權衰減因子對演算法的有效濾波有很大的影響，當目標快轉彎時,會出現大的誤差,這時候可以通過改變機動檢測門限來減小。
相關文章
- javascript元素勻速運動程式碼例項JavaScript
- canvas水平勻速運動效果Canvas
- javascript封裝動畫函式（勻速、變速）JavaScript封裝動畫函式
- 圓形小球環形均勻分佈程式碼例項
- 空間、運動（時間）以及程式設計師程式設計師
- 使用python玩轉二維碼！速學速用！⛵Python
- 關於自動化運維的思考-基線運維
- 基於AMC4030的二維滑軌的MATLAB程式設計控制Matlab程式設計
- 基於紅外和超聲波的手動/自動調速風扇系統
- Cassandra如何配置可實現節點間資料量均勻分配
- 基於混合高斯模型的運動目標檢測演算法模型演算法
- 【TSPITR】RMAN表空間基於時間點的自動恢復
- flex產品列表均勻分佈程式碼例項Flex
- three.js 製作屬於自己的動態二維碼JS
- BAT新風向標：程式設計師有福利了！（速點，免費送）BAT程式設計師
- 動態二維碼免費製作
- 獨闢蹊徑：基於產品思維驅動運維自動化建設運維
- 3D遊戲-作業三-空間與運動3D遊戲
- 利用OpenCV生成關於某點的顏色徑向均勻漸變影象OpenCV
- 伊蘭特汽車怠速時發動機異響的原因分析
- AverageTextView——均勻顯示的TextViewTextView
- css多個元素在容器內水平均勻分佈CSS
- 基於YOLOv4的目標檢測系統（附MATLAB程式碼+GUI實現）YOLOMatlabGUI
- [微信小程式系列] 動畫案例之圓點沿著圓圈運動微信小程式動畫
- 程式設計師小抄——GitHub 熱點速覽 Vol.44程式設計師Github
- GitHub 官方大動作頻頻「GitHub 熱點速覽 v.22.24」Github
- vPaaS低程式碼音視訊工廠：極速智造，永珍空間
- js圓形環繞運動程式碼例項JS
- 基於YOLOv5的目標檢測系統詳解（附MATLAB GUI版程式碼）YOLOMatlabGUI
- 如何用MFC畫出直線、虛線、折線、圓、橢圓、矩形、弧形（附上原始碼）原始碼
- 運維前線：一線運維專家的運維方法、技巧與實踐1.3 運維自動化的困境和價值運維
- css水平元素寬度自適應均勻排列程式碼例項CSS
- 使用oracle procedure儲存過程自動擴充套件表空間空間tablespace_自動化運維Oracle儲存過程套件運維
- 專案管理思維和工作生活-目標驅動專案管理
- 有趣的CSS題目（6）： 全相容的多列均勻佈局問題CSS
- 沉浸式投影空間的製作都具備的特點分析
- 空間研究 | 基於移動定位大資料的城市空間研究進展大資料
- 深度學習之影像目標檢測速覽深度學習