在扩展卡尔曼滤波器中计算状态空间模型和测量模型的雅可比矩阵

Posted 2023-02-21

【中文标题】在扩展卡尔曼滤波器中计算状态空间模型和测量模型的雅可比矩阵

【英文标题】：Calculating Jacobian matrix for state space model and measurement model in extended kalman filter

【发布时间】：2021-01-20 14:42:22

【问题描述】：

我正在使用等速模型来估计卡尔曼滤波器中的位置。测量值是使用 randn 函数在 Matlab 中模拟生成的，该函数将添加到同样在 Matlab 上生成的地面实况数据中。而且由于它的雷达测量基本上将具有角度和范围。为了计算笛卡尔坐标系中的位置，我使用了明显非线性的 sin 和 cos 函数。使用简单的卡尔曼滤波器，我看到我的修正几乎是在测量之后进行的。所以我打算转向扩展卡尔曼滤波器。由于状态空间模型仍然是线性的，但测量结果是非线性的（因为 sin 和 cos）。我应该得到测量矩阵的雅可比矩阵吗？我在一个 Github 页面（https://github.com/mithi/fusion-ekf-python）中看到了类似的实现。但我不确定雅可比是如何在那里计算的，代码 sn-p 看起来像这样

r11 = px / d
r12 = py / d
r21 = -py / d_squared
r22 = px / d_squared
r31 = py * (vx * py - vy * px) / d_cubed
r32 = px * (vy * px - vx * py) / d_cubed

H = np.matrix([[r11, r12, 0, 0], 
              [r21, r22, 0, 0], 
              [r31, r32, r11, r12]])

其中 px ,py 是位置，vx 和 vy 是 x 和 y 轴上的速度。

谁能告诉我这里是如何计算雅可比行列式的，如果我只使用雅可比行列式作为测量矩阵 H 是否可以使用 KG= PH(transpose)/HPH(transpose) 方程进行卡尔曼增益计算

谢谢

比什玛

【参考方案1】：

在我看来，您给出的示例是范围、方位和径向速度。既然你只想要范围和方位，你应该扔掉第三行。

一个问题是：观察的单位是什么？我假设范围（如位置状态）以米为单位，范围以弧度为单位。更改这些很简单：例如，如果您的方位以度为单位，您应该将与下方方位相关的所有内容（计算值、其导数的分量）乘以 180/pi。

一个稍微微妙的问题是：轴承的意义是什么？也就是说，它们是从 x 轴逆时针测量（在数学中很常见），还是从 y 轴顺时针测量（如果我们认为 y 轴为北，x 轴为东，这在导航中很常见）。

我将在这里使用导航约定，并使用 as 来说明目标的位置 (n,e)，给出目标的北 (n) 和东 (e) 距离雷达传感器的距离，以及它的速度 (u,v) (u 北, v 东)。但是测量值仅取决于 n 和 e，因此不再提及 u 或 v。

最后一个问题是：目标是否离传感器足够远，以至于我们不应该像上面那样使用平面地球模型？我假设我们可以忽略地球曲率。

我们的测量，就位置状态而言

range = sqrt( e*e + n*n)
brng = atan2( e, n)

我们还需要雅可比行列式，即 range 和 brng 对 n 和 e 的导数。 （实际上我们也想要 u 和 v 的导数，但是对于范围和方位，这些都是 9）。所以我们想要

drangebydn
drangebyde
dbrngbydn
dbrngbyde

有多种方法可以解决这个问题。一种是手工进行微分并编码结果；另一种是使用“自动微分”（如果实现语言支持的话）；最后的手段是尝试以数字方式进行。

这是我手工操作的尝试（未经测试）： 范围的方程很简单，但 atan2 有点棘手。通常数学书只谈论 atan，并不完全相同。在这种情况下进行计算的一种方法是考虑反问题：就 r 和 b 而言，我们有

n = r*cos(b)
e = r*sin(b)

这些很容易区分，我得到 r,b->n,e 的 Jacobian 为

( n/r  -e )
( e/r  n  )

我们想要 n,e -> r,b 的雅可比行列式；这是上面矩阵的逆矩阵。所以最后：

Let r = range(n,e)
drangebydn = n/r 
drangebyde = e/r 
dbrngbydn = -e/(r*r) 
drangebyde = n/(r*r) 

我已经做了很多这样的事情，并且（由于浪费了很多时间，并且掉了很多头发）我总是单独测试衍生品。使用导数的过滤器和估计器很难调试；通过始终单独测试这些位，您将节省时间和心痛。一个捕获许多常见错误的简单测试是使用，如果我们有一个函数 f（例如 range(n,e) 用于固定 e）及其导数 fdot（例如 drangebydn），则 fdot 的积分在区间 n0,n1 上是 f(n1)-f(n0) 的差值。可以进行数值积分。