?? testcv.asv
字號:
x1_position=[];
x1radar_position=[];
x1_0=[7700,5600,9700,180,40,55,0,0,0]'; %運動軌跡仿真初始值1
[x1radar_position,x1_position,x1_state]=track_cv(x1_0,1.5,60,0.2); %目標1在[0,90s]作CV運動
x2Dradar_position=x1radar_position(1:2,:);
[x2radar_position,x2_position,x2_state]=track_cv(x1_0,1.5,60,0.0001);
xIF_position=x2radar_position(2:3,:);
%卡爾曼濾波
x=[7700,5600,9700,180,40,55]';
P=1000*[eye(6)];
F=[eye(3),diag([1.5 1.5 1.5]);
zeros(3),eye(3)];
G=[diag([1.5 1.5 1.5]);
eye(3)];
Cw=[diag([0.001^2 0.001^2 0.001^2])];
Cn=[10^2 0 0;0 0.05^2 0;0 0 0.05^2];
Cn1=[10^2 0 0;0 0.05^2 0];
Cn2=[0 0.05^2 0;0 0 0.05^2];
Fradar_position=[];
for i=1:60
x1=F*x;
P=F*P*F'+G*Cw*G';
H=[x1(1)/sqrt(x1(1)^2+x1(2)^2+x1(3)^2) x1(2)/sqrt(x1(1)^2+x1(2)^2+x1(3)^2) x1(3)/sqrt(x1(1)^2+x1(2)^2+x1(3)^2) 0 0 0;
-x1(2)/(x1(1)^2+x1(2)^2) x1(1)/(x1(1)^2+x1(2)^2) 0 0 0 0;];
K=P*H'*inv(H*P*H'+Cn1);
P=(eye(6)-K*H)*P;
xtemp=x1+K*(x2Dradar_position(:,i)-[sqrt(x1(1)^2+x1(2)^2+x1(3)^2);atan(x1(2)/x1(1))]);
H2=[-x1(2)/(x1(1)^2+x1(2)^2) x1(1)/(x1(1)^2+x1(2)^2) 0 0 0 0;
-(x1(1)*x1(3))/(sqrt(x1(1)^2+x1(2)^2)*(x1(1)^2+x1(2)^2+x1(3)^2)) -(x1(2)*x1(3))/(sqrt(x1(1)^2+x1(2)^2)*(x1(1)^2+x1(2)^2+x1(3)^2)) sqrt(x1(1)^2+x1(2)^2)/(x1(1)^2+x1(2)^2+x1(3)^2) 0 0 0];
K2=P*H2'*inv(H2*P*H2'+Cn2);
xtemp=xtemp+K2*(xIF_position(:,i)-[atan(x1(2)/x1(1));atan(x1(3)/sqrt(x1()))]);
Fradar_position=[Fradar_position x];
end
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -