集群的三维比例制导律、目标非线性探测跟踪滤波及其MATLAB/Python仿真代码
来源：http://worthpen.top/#/home/blog?blog=pot-blog4.md
目录
- 引言
- 制导
-
- 动力学模型
- 比例导引法
- MATLAB代码
- Python代码
- 目标跟踪
-
- 探测模型
- MATLAB代码
引言
本文给出了集群的三维比例导引与非线性目标跟踪滤波。但在制导过程中节点间无交互，各自使用比例导引，可以在主程序上修改后使用协同导引方法。在探测过程中节点存在交互，使用序贯融合滤波算法。基本的滤波算法为无迹Kalman滤波。
制导
比例导引是导弹制导的方法之一。所谓比例导引法是指导弹在向目标接近的过程中，使导弹的速度向量V在空间的转动角速度正比于目标视线的转动角速度。本文首先建立了三自由度动力学模型，随后给出了制导律算法。
动力学模型
{ d V d t = P − X − m g s i n ( θ ) m d θ d t = n y 2 g − m g c o s ( θ ) V d φ d t = − n z 2 g V c o s ( θ ) d x d t = V c o s ( θ ) c o s φ d y d t = V s i n ( θ d z d t = − V c o s ( θ ) sin ( φ ) \left\{ \begin{matrix} \frac{dV}{dt} = \frac{P - X - mgsin(\theta)}{m} \\ \frac{d\theta}{dt} = \frac{n_{y2}g - mgcos(\theta)}V \\ \frac{d\varphi}{dt} = - \frac{n_{z2}g}{Vcos(\theta)} \\ \frac{dx}{dt} = Vcos(\theta)cos\varphi \\ \frac{dy}{dt} = Vsin(\theta \\ \frac{dz}{dt} = - Vcos(\theta){\sin(\varphi)} \\ \end{matrix} \right. ⎩ ⎨ ⎧dtdV=mP−X−mgsin(θ)dtdθ=Vny2g−mgcos(θ)dtdφ=−Vcos(θ)nz2gdtdx=Vcos(θ)cosφdtdy=Vsin(θdtdz=−Vcos(θ)sin(φ)
式中， P P P为推力， m m m为质量，两者均为与时间相关的函数，g为重力加速度， n y 2 n_{y2} ny2和 n z 2 n_{z2} nz2与分别为法向过载与侧向过载。
仿真过程中暂不考虑执行机构延迟，给定过载信号经过饱和环节后直接给入动力学模型进行解算。
比例导引法
比例导引法的制导指令如下：
{ n y 2 = N ∣ r ˙ ∣ q ˙ y g + cos ( θ ) n z 2 = N c o s ( q y ) ∣ r ˙ ∣ q ˙ z g \left\{ {\begin{matrix} {n_{y2} = \frac{N\left| \overset{˙}{r} \right|{\overset{˙}{q}}_{y}}{g} + {\cos(\theta)}} \\ {n_{z2} = \frac{Ncos\left( q_{y} \right)\left| \overset{˙}{r} \right|{\overset{˙}{q}}_{z}}{g}} \\ \end{matrix}} \\ \right. ⎩ ⎨ ⎧ny2=gN r˙ q˙y+cos(θ)nz2=gNcos(qy) r˙ q˙z
MATLAB代码
四个飞行器分别对一个目标进行制导。
主程序：（完整代码见文末）
function [ny2c,nz2c,tgo]=guid_law_P(target,missile,loc_other)
xt=target(4);yt=target(5);zt=target(6);
dxt=target(1)*cos(target(2))*cos(target(3));
dyt=target(1)*sin(target(2));
dzt=-target(1)*cos(target(2))*sin(target(3));
x=missile(4);y=missile(5);z=missile(6);
dx=missile(1)*cos(missile(2))*cos(missile(3));
dy=missile(1)*sin(missile(2));
dz=-missile(1)*cos(missile(2))*sin(missile(3));
xr=xt-x;yr=yt-y;zr=zt-z;xr_v=dxt-dx;yr_v=dyt-dy;zr_v=dzt-dz;
g0=9.81;
R=6371000;
N=4;
dr_mo=sqrt(xr_v^2+yr_v^2+zr_v^2);
g=g0*(R^2)/((R+y)^2);
r_relative=sqrt(xr^2+yr^2+zr^2);
tgo=r_relative/dr_mo;
qy_v=((xr^2+zr^2)*yr_v-yr*(xr*xr_v+zr*zr_v))/((xr^2+yr^2+zr^2)*sqrt(xr^2+zr^2));
qz_v=(zr*xr_v-xr*zr_v)/(xr^2+zr^2);
qy=atan(yr/sqrt(xr^2+zr^2));
ny2c=N*dr_mo*qy_v/g+cos(missile(2));
nz2c=-N*cos(qy)*dr_mo*qz_v/g;
end
Python代码
四个飞行器分别对一个目标进行制导。
主程序：（完整代码见文末）
def guid_law_P(target, missile):
xt = target[3]
yt = target[4]
zt = target[5]
dxt = target[0] * math.cos(target[1]) * math.cos(target[2])
dyt = target[0] * math.sin(target[1])
dzt = -target[0] * math.cos(target[1]) * math.sin(target[2])
x = missile[3]
y = missile[4]
z = missile[5]
dx = missile[0] * math.cos(missile[1]) * math.cos(missile[2])
dy = missile[0] * math.sin(missile[1])
dz = -missile[0] * math.cos(missile[1]) * math.sin(missile[2])
xr = xt - x
yr = yt - y
zr = zt - z
xr_v = dxt - dx
yr_v = dyt - dy
zr_v = dzt - dz
g0 = 9.81
R = 6371000
N = 2
dr_mo = math.sqrt(xr_v**2+yr_v**2+zr_v**2)
g = g0 * (R ** 2) / ((R + y) ** 2)
# r_relative = math.sqrt(xr ** 2 + yr ** 2 + zr ** 2)
qy_v = ((xr ** 2 + zr ** 2) * yr_v - yr * (xr * xr_v + zr * zr_v)) / ((xr ** 2 + yr ** 2 + zr ** 2) * math.sqrt(xr ** 2 + zr ** 2))
qz_v = (zr * xr_v - xr * zr_v) / (xr ** 2 + zr ** 2)
qy=math.atan(yr/math.sqrt(xr**2+zr**2))
ny2c = N * dr_mo * qy_v / g + math.cos(qy)
nz2c = -N * math.cos(qy) * dr_mo * qz_v / g
return [ny2c, nz2c]
目标跟踪
目标跟踪采用融合无迹Kalman滤波算法。融合方法为序贯滤波。在融合过程中，考虑了节点间的延迟，使用了基于异步量测的异步无迹Kalman滤波算法。
探测模型
多源信息融合过程中，需要将探测值由传感器坐标系下的值转化为惯性坐标系下的值，同时为了便于表达，我们首先建立惯性坐标系下的传感器探测模型。惯性坐标系下传感器获得的信息为目标在惯性坐标系下的位置x, y, z与速度vx, vy, vz。传感器的测量方程为
其中，y为传感器的量测，
为观测矩阵；v是传感器的量测噪声，为零均值高斯白噪声，协方差矩阵为R。
接下来，我们对传感器坐标系下的传感器量测模型进行建模。传感器负责得到目标的原始信息并将目标信息发送至信息处理模块进行滤波估计。传感器的测量方程为
其中，y为传感器的量测，f为在传感器坐标系下目标在t时刻的状态x(t)的理想量测，即
其中，R表示传感器与目标的距离，phi表示目标相对于传感器的方位角，theta表示目标相对于传感器的俯仰角。v(t)是传感器的量测噪声，为零均值高斯白噪声，协方差矩阵为R。假定不同方向的量测噪声互不相关。
MATLAB代码
滤波结果如图。
主程序：（完整代码见文末）
classdef unscented_kf_asyn < handle
properties
nth
t_step
measure_noise_mat
measure_func
est_data
est_line
estimate_last
cov_last
est_data_noise
num_t
num_state
end
methods
function obj = unscented_kf_asyn(nth, num_t, t_step, num_state,...
measure_noise_mat, measure_func, est_figure)
obj.nth = nth;
obj.t_step = t_step;
obj.est_data = zeros(num_state, num_t);
obj.est_data_noise = zeros(num_state, num_t);
obj.measure_noise_mat = measure_noise_mat;
obj.measure_func = measure_func;
obj.estimate_last = [];
obj.cov_last = [];
figure(est_figure.figure_handle)
obj.est_line = plot(0, 0, '*');
est_figure.add_line(obj.est_line, strcat('导弹', num2str(nth), 'x位置估计'))
hold on
obj.num_t = num_t;
obj.num_state = num_state;
end
function filtering(obj, cou_t, measure, self_loc, skip_pred, ...
target_loc, save, delay)
if size(obj.estimate_last,1)==0 && size(measure, 2)~=0
[obj.estimate_last, obj.cov_last] = obj.init(measure(1:3, :), self_loc);
else
[obj.estimate_last, obj.cov_last] = obj.core(measure, ...
self_loc, obj.estimate_last, obj.cov_last, skip_pred, delay);
end
if save
obj.est_data(:, cou_t) = obj.estimate_last;
obj.est_data_noise(:, cou_t) = abs(obj.estimate_last-target_loc);
set(obj.est_line, 'XData', (0:cou_t-1)*obj.t_step+1,...
'YData', obj.est_data_noise(1, 1:(cou_t)));
end
end
function [estimate, cov_error_estimate] = core(obj, measure, ...
self_loc, forecast, P_forecast, skip_pred, delay)
% 状态转移矩阵
A = [1,0,0,obj.t_step,0,0;
0,1,0,0,obj.t_step,0;
0,0,1,0,0,obj.t_step;
0,0,0,1,0,0;
0,0,0,0,1,0;
0,0,0,0,0,1;
];
% 系统噪声系数矩阵
gama = [0.5*obj.t_step*obj.t_step,0,0;
0,0.5*obj.t_step*obj.t_step,0;
0,0,0.5*obj.t_step*obj.t_step;
obj.t_step,0,0;
0,obj.t_step,0;
0,0,obj.t_step];
% 系统噪声协方差
Q = 1*[1,0,0;
0,1,0;
0,0,1];
if ~skip_pred
forecast = A * forecast;
P_forecast = (A * P_forecast * A'+ gama * Q * gama');
end
if size(measure, 2)==0
estimate=forecast;
cov_error_estimate = P_forecast;
else
num_state = size(forecast,1);
Q_td_t = Q_t1_t2(delay, obj.t_step);
[w_td2t_t_1_point,~] = ...
obj.get_point(zeros(num_state,1), Q_td_t,3);
[last_est_point,last_est_weight] = ...
obj.get_point(forecast, P_forecast, 3);
measure_t_1_point = zeros(size(measure,1),size(last_est_point,2));
for cou_point = 1:size(last_est_point,2)
measure_t_1_point(:,cou_point)=obj.measure_func(...
self_loc, last_est_point(:,cou_point));
end
measure_expect_t_1 = sum(last_est_weight(1,:).*measure_t_1_point,2);
cov_zt_t_1 = last_est_weight(2,:).*(measure_t_1_point-...
measure_expect_t_1)*(measure_t_1_point-measure_expect_t_1)'...
+obj.measure_noise_mat;
% TODO 应该修正R后的point
cov_w_td2t_zt_t_1= last_est_weight(2,:).*(w_td2t_t_1_point)*...
(measure_t_1_point-measure_expect_t_1)';
w_td2t = cov_w_td2t_zt_t_1/cov_zt_t_1*(forecast(1:3,:)-...
measure_expect_t_1);
cov_w_td2t_t = Q_td_t-cov_w_td2t_zt_t_1/cov_zt_t_1*...
cov_w_td2t_zt_t_1';
[forecast_point,forecast_weight]=obj.get_point(forecast,P_forecast,3);
% (3-25)
backcast = A_t1_t2(obj.t_step,delay,A)*(forecast-w_td2t);
P_backcast = A_t1_t2(obj.t_step,delay,A)*P_forecast*...
(A_t1_t2(obj.t_step,delay,A))'-A_t1_t2(obj.t_step,delay,A)*...
(cov_w_td2t_t)*(A_t1_t2(obj.t_step,delay, A))';
[backcast_point,backcast_weight]=obj.get_point(backcast,P_backcast,3);
measure_expect_t_point = zeros(size(measure,1),size(backcast_point,2));
for cou_point = 1:size(backcast_point,2)
measure_expect_t_point(:,cou_point)=obj.measure_func(...
self_loc, backcast_point(:,cou_point));
end
measure_expect_t = sum(backcast_weight(1,:).*measure_expect_t_point,2);
% (3-36)
P_z = backcast_weight(2,:).*(measure_expect_t_point-...
measure_expect_t)*(measure_expect_t_point-measure_expect_t)'...
+obj.measure_noise_mat;
% (3-37)
% TODO 应该修正R后的point
P_xz = forecast_weight(2,:).*(forecast_point-forecast)*...
(measure_expect_t_point-measure_expect_t)';
kalman_gain = P_xz/P_z;
estimate = forecast + kalman_gain*(measure - measure_expect_t);
cov_error_estimate = P_forecast - P_xz/P_z*P_xz';
end
end
function [output1,output2] = get_point(~, x, P, n)
% 得到无迹变换的点
% 输入：状态估计，误差协方差，量测数（决定点数量，点数量为2n+1）
% 输出：点的矩阵,权重矩阵
% alpha = 0.01;
% ka =0;
% beta=2;
%%
% lam = alpha^2*(n+ka)-n;
lam = 3-n;
output1=zeros(size(x,1),2*n+1);
% 第一行表示一阶统计特性的权重，第二行表示二阶统计特性的权重
output2=zeros(2,2*n+1);
sqrt_P = ((n+lam)*P)^0.5;
if ~isreal(sqrt_P)
sqrt_P=real(sqrt_P);
end
output1(:,2*n+1)=x;
% output2(:,2*n+1)=[lam/(n+lam);
% lam/(n+lam)+1-alpha^2+beta];
output2(:,2*n+1)=[lam/(n+lam);
lam/(n+lam)];
for cou_point = 1:n
output1(:,cou_point)=x+sqrt_P(:,cou_point);
output2(:,cou_point)=0.5/(n+lam);
output1(:,cou_point+n)=x-sqrt_P(:,cou_point);
output2(:,cou_point+n)=0.5/(n+lam);
end
end
function [estimate, cov_error_estimate] = init(~, measure, self_loc)
% 初始化取量测值对应的状态为当前时刻估计，9倍的量测误
%差协方差阵为当前时刻估计误差协方差。
% 此处为9的原因是3倍的标准差为百分之95的可能的误差值，
%可以认为包含了最大误差的情况。
estimate=[self_loc(1:3,1);0;0;0]+sightline2rect(measure);
cov_error_estimate = 30*[1,0,0,1,0,0;
0,1,0,0,1,0;
0,0,1,0,0,1;
1,0,0,1,0,0;
0,1,0,0,1,0;
0,0,1,0,0,1];
end
end
end
function [out]=Q_t1_t2(t1,t2)
% 仅对应于
% A = [1, 0, 0, t_step, 0, 0;
% 0, 1, 0, 0, t_step, 0;
% 0, 0, 1, 0, 0, t_step;
% 0, 0, 0, 1, 0, 0;
% 0, 0, 0, 0, 1, 0;
% 0, 0, 0, 0, 0, 1];
%gama = [0.5*t_step*t_step,0,0;
% 0,0.5*t_step*t_step,0;
% 0,0,0.5*t_step*t_step;
% t_step,0,0;
% 0,t_step,0;
% 0,0,t_step];
%时的模型，其中t为t_step
%%
t=t2-t1;
out = [9/20*t^5, 0, 0, 3/8*t^4, 0, 0;
0, 9/20*t^5, 0, 0, 3/8*t^4, 0;
0, 0, 9/20*t^5, 0, 0, 3/8*t^4;
3/8*t^4, 0, 0, 1/3*t^3, 0, 0;
0, 3/8*t^4, 0, 0, 1/3*t^3, 0;
0, 0, 3/8*t^4, 0, 0, 1/3*t^3];
end
function [out]=A_t1_t2(t1,t2, A)
if (t1-t2)<=0
out = A;
else
out = inv(A);
end
end
完整代码分为多个文件，数量较多，无法放在文章中。完整代码在公众号（沸腾的火锅资源号）中自取。