集群的三维比例制导律、目标非线性探测跟踪滤波及其MATLAB/Python仿真代码

BoilingHotPot

已于 2024-10-17 16:49:35 修改

阅读量1.7k

点赞数

文章标签： matlab python 开发语言

于 2023-06-04 23:58:40 首次发布

本文链接：https://blog.csdn.net/boilinghotpot/article/details/130241870

版权

来源：https://maiya.fan/blog?id=6566173b6aa58e39d930190d

引言

本文给出了集群的三维比例导引与非线性目标跟踪滤波。但在制导过程中节点间无交互，各自使用比例导引，可以在主程序上修改后使用协同导引方法。在探测过程中节点存在交互，使用序贯融合滤波算法。基本的滤波算法为无迹Kalman滤波。

制导

比例导引是导弹制导的方法之一。所谓比例导引法是指导弹在向目标接近的过程中，使导弹的速度向量V在空间的转动角速度正比于目标视线的转动角速度。本文首先建立了三自由度动力学模型，随后给出了制导律算法。

动力学模型

$\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.$
式中， $P$ 为推力， $m$ 为质量，两者均为与时间相关的函数，g为重力加速度， $n_{y2}$ 和 $n_{z2}$ 与分别为法向过载与侧向过载。
仿真过程中暂不考虑执行机构延迟，给定过载信号经过饱和环节后直接给入动力学模型进行解算。

比例导引法

比例导引法的制导指令如下：
$\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.$

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。

接下来，我们对传感器坐标系下的传感器量测模型进行建模。传感器负责得到目标的原始信息并将目标信息发送至信息处理模块进行滤波估计。传感器的测量方程为
(2-3)
其中，y为传感器的量测，f为在传感器坐标系下目标在t时刻的状态x(t)的理想量测，即
(2-4)
其中，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