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

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

引言

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

制导

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

动力学模型

$$\{\begin{array}{c}\frac{dV}{dt}=\frac{P-X-mgsin(\theta )}{m}\\ \frac{d\theta }{dt}=\frac{n_{y2}g-mgcos(\theta )}{V}\\ \frac{d\phi }{dt}=-\frac{n_{z2}g}{Vcos(\theta )}\\ \frac{dx}{dt}=Vcos(\theta )cos\phi \\ \frac{dy}{dt}=Vsin(\theta \\ \frac{dz}{dt}=-Vcos(\theta )\sin (\phi )\end{array}$$
式中，$P$为推力，$m$为质量，两者均为与时间相关的函数，g为重力加速度，$n_{y2}$和 $n_{z2}$与分别为法向过载与侧向过载。
仿真过程中暂不考虑执行机构延迟，给定过载信号经过饱和环节后直接给入动力学模型进行解算。

比例导引法

比例导引法的制导指令如下：
$$\{$$