14、移动目标跟踪中的成本函数计算与最优控制输入分析

移动目标跟踪中的成本函数计算与最优控制输入分析

1. MATLAB 实现成本函数计算

在移动目标跟踪的场景中，我们可以使用 MATLAB 的符号计算功能来获取成本函数 (J) 关于初始条件的表达式。对 (J) 关于 (\theta) 求导，可得到如下方程：
[
\frac{dJ}{d\theta}= 0 \Rightarrow -a_{\triangle}\cos \theta^{ }+ b_{\triangle}\sin \theta^{ }= 0 \Rightarrow \tan \theta^{ }=\frac{u_y(0)(\Delta t)^2 + 3v_y(0)\Delta t + 2y_a(0) - 2y_t(0)}{u_x(0)(\Delta t)^2 + 3v_x(0)\Delta t + 2x_a(0) - 2x_t(0)} = \frac{a_{\triangle}}{b_{\triangle}}
]
其中，(\theta^{ }) 是在给定配置下目标的最坏方向。需要注意的是，(a_{\triangle}) 前面有负号。在使用 coeffs() 函数时要格外小心，该函数返回的系数是按从低到高的顺序排列的。例如，对于函数 (2\cos \theta - 3\sin \theta + 5)，使用 coeffs() 函数返回的数组为：

>> syms theta real;
>> f = 2*cos(theta) - 3*sin(theta) + 5;
&g