钢铁侠盔甲要是真的像电影里一样以一敌百，还要具备发射导弹的功能，现在军工领域已经不再是简简单单的发射导弹那么简单了，具有跟踪功能的导弹比比皆是，导弹的自动导引制导是指导弹能够自主地搜索、捕获、识别、跟踪和攻击目标的制导方式，这是制导武器系统最主要的现代制导体制。
本节课就让我们揭开导弹在三维空间的制导算法。
今天下午天气晴朗，刚刚从松花江上划船回来，Dsy就拿来了一本书周荻著的《寻的导弹新型导引规律》（自动导引制导的导弹又叫寻的导弹），你兴奋的顾不得休息，把大神Dsy拉到书桌前，让他赶紧为你揭晓导弹制导的算法。
Dsy笑着对你说，先别急，有了前几天的知识，理解导弹指导的内容并不难，先看下图，理解下导弹制导的原理，在笛卡尔坐标系下导弹Missile与打击物Target的距离为r。
由上图很容易发现导弹制导实际上就是在一个三维平面x-y-z内运动的质点Target(x_t,y_t,z_t)相对于Missile(x_m,y_m,z_m)的位置检测和估计的问题，这类问题正符合kalman解决问题的范围。
把三维问题分解到二维空间，定义状态空间：
状态转移矩阵如下：
上述两个矩阵，分别描述的状态空间与状态转移矩阵，乘到一起就是运动学入门的距离和速度加速度相关的公式。再看下图整个导弹制导的常规滤波流程，有没有发现很熟悉。
虽然有些符号使用不同，但细心比对下就会发现，整个卡尔曼算法的流程和前几节课是一样的，下面我们通过实例化将2D空间扩展到3D空间，编写导弹制导的kalman滤波的matlab程序。
在笛卡尔坐标系下，目标的质点运动方程为：
在不考虑复杂干扰因子的情况下，该状态方程为线性：
其中：
观测方程为：
在导弹对目标方位的测量为俯仰角和水平方向偏向角。
A=[eye(3),delta_t*eye(3),(exp(-1*longa*delta_t)+...
longa*delta_t-1)/longa^2*eye(3);
zeros(3),eye(3),(1-exp(-1*longa*delta_t))/longa*eye(3);
zeros(3),zeros(3),exp(-1*longa*delta_t)*eye(3)];
Xn=F*ex+G*u;
Zn=[atan(Xn(2)/sqrt(Xn(1)^2+Xn(3)^2) ),atan(-1*Xn(3)/Xn(1))]';
P=F*P0*F'+Q;
dh1_dx=-1*Xn(1)*Xn(2)/(Xn(1)^2+Xn(2)^2+Xn(3)^2)/sqrt(Xn(1)^2+Xn(3)^2);
dh1_dy=sqrt(Xn(1)^2+Xn(3)^2)/(Xn(1)^2+Xn(2)^2+Xn(3)^2);
dh1_dz=-1*Xn(2)*Xn(3)/(Xn(1)^2+Xn(2)^2+Xn(3)^2)/sqrt(Xn(1)^2+Xn(3)^2);
dh2_dx=Xn(3)/(Xn(1)^2+Xn(3)^2);
dh2_dy=0;
dh2_dz=-1*Xn(1)/(Xn(1)^2+Xn(3)^2);
H=[dh1_dx,dh1_dy,dh1_dz,0,0,0,0,0,0;dh2_dx,dh2_dy,dh2_dz,0,0,0,0,0,0];
K=P*H'/(H*P*H'+R);
ex=Xn+K*(z-Zn);
P0=(eye(9)-K*H)*P;
--------------------------------------------------------------------------------------
仿真结果如下：
了解的导弹制导的kalman滤波方式，相信你可以更加熟练的将kalman应用到各种相关的领域了。
1. EXTENDED KALMAN FILTER DESIGN USING BEARING AND TIME-TO-GOMEASUREMENT FOR A HOMING MISSILE GUIDANCE
2. 《寻的导弹新型导引规律》
长按下图，识别图中二维码，关注“泡泡机器人SLAM”（ID：paopaorobot_slam）公众号。系列全文PDF下载链接在【泡泡机器人原创专栏】第 5 课 UKF 与粒子滤波实现中给出，敬请关注。
欢迎来泡泡机器人论坛交流讨论学习公开课过程中遇到的问题：
www.paopaorobot.org
【版权声明】泡泡机器人SLAM的所有文章全部由泡泡机器人的成员花费大量心血制作而成的原创内容，希望大家珍惜我们的劳动成果，转载请务必注明出自【泡泡机器人SLAM】微信公众号，否则侵权必究！同时，我们也欢迎各位转载到自己的朋友圈，让更多的人能进入到SLAM这个领域中，让我们共同为推进中国的SLAM事业而努力！
【注】商业转载请联系刘富强（[email protected]）进行授权。普通个人转载，请保留版权声明，并且在文章下方放上“泡泡机器人SLAM”微信公众账号的二维码即可。
【编辑】徐武民
