算法描述

考虑离散时间非线性动态系统
$$\{\begin{array}{c}{x}_{k+1}=f_k(x_k,w_k)\\ z_k=h_k(x_k,v_k)\end{array}$$
其中是$k$时间指标，$x_k$是$k$时刻系统状态向量，$f_k(\bullet )$是系统状态演化映射，$w_k$是过程演化噪声，$z_k$是$k$时刻对系统状态的量测向量，$h_k(\bullet )$是量测映射，$v_k$是为量测噪声。一般假设过程噪声和量测噪声均服从高斯分布，即$w_k\sim N(0,Q_k),v_k\sim N(0,R_k)$,其中$Q_k,R_k$表示协方差矩阵。

​ 扩展卡尔曼滤波算法实质上是一种线性化算法，它先将系统中涉及到非线性映射进行线性化处理，主要是利用泰勒级数作线性近似,泰勒级数（Taylor Series）是将一个函数在某点附近展开成无穷级数的方法。对于一个在点 $a$ 处无限可微的函数 $f(x)$，其泰勒级数展开式为：
$$f(x)=\sum _{n=0}^{\infty }\frac{f^{(n)}(a)}{n!}(x-a{)}^n$$

其中：

• $f^{(n)}(a)$表示函数 $f$ 在点 $a$处的 $n$ 阶导数，
• $n!$是$n$ 的阶乘，
• $(x-a{)}^n$是 $(x-a)$ 的 $n$次幂。

​ 扩展卡尔曼滤波一般是泰勒级数的一阶或者二阶近似

• 一阶近似
  $$f(x)\approx f(a)+f^{\prime }(a)(x-a)$$

• 二阶近似
  $$f(x)\approx f(a)+f^{\prime }(a)(x-a)+\frac{f^{\prime \prime }(a)}{2!}(x-a{)}^2$$

​ 一阶的EKF算法的步骤描述如下

1. 假定已知$k$时刻状态向量$\hat{x}(k|k)$及其协方差矩阵$P(k|k)$

2. 构建两个雅可比矩阵，这里为$f_k(\bullet ),h_k(\bullet )$多维函数，$x$为多维向量。
   $$\begin{array}{c}{F}_{k+1|k}=\frac{\partial f_k(x)}{\partial x}{|}_{x=x_{k|k}}\\ H_{k+1}=\frac{\partial h_k(x)}{\partial x}{|}_{x=x_{k+1|k}}\end{array}$$

3. 按照状态演化映射$f_k(\bullet )$,计算状态的一步预测$\hat{x}(k+1|k)=f_k(\hat{x}(k|k))$,同时计算出协方差矩阵的一步预测$P(k+1|k)=F_{k+1|k}P(k|k)F_{k+1|k}^T+Q(k)$

4. 计算卡尔曼增益
   $$K(k+1)=\frac{P(k+1|k)H_{k+1}^T}{H_{k+1}P(k+1|k)H_{k+1}^T+R(k+1)}$$

5. 按照量测映射计算量测的预测值$\hat{z}(k+1|k)=h_k(\hat{x}(k+1|k))$

6. 更新状态向量及协方差矩阵，具体公式如下
   $$\begin{array}{c}\hat{x}(k+1|k+1)=\hat{x}(k+1|k)+K(k+1)\left[z(k+1)-\hat{z}(k+1|k+1)\right]\\ P(k+1|k+1)=P(k+1|k)-K(k+1)H_{k+1}P(k+1|k)\end{array}$$
   其中$z(k+1)$表示$k+1$时刻的实际量测值。

实际案例

现在考虑一个二维平面下的一个双观测站的目标跟踪系统，如下图所示

​ 假设两个观测站S1,S2,它们自身位置分别为$S1(x_1,y_1),S2(x_2,y_2)$，而每一时刻观测站能够采集到目标相对于自身的方位角，现需要利用收集的方位角信息对目标进行跟踪。利用数学关系，我们能够得到如下关系
$$\{\begin{array}{c}{\theta }_{1,t}=atan(\frac{y_t-y_1}{x_t-x_1})\\ {\theta }_{2,t}=atan(\frac{y_t-y_2}{x_t-x_2})\end{array}$$
在仿真实验中我们设定目标做匀速直线运动，目标初始位置为(100,100),速度大小为(10,10),而两个观测站的位置分别为(0,0),(1000,0)。选取状态向量为$(x,y,\dot{x},\dot{y})$,其中$x,y$表示目标在x,y方向上位置信息，$\dot{x},\dot{y}$表示目标在x，y方向上的速度信息，选择匀速模型作为状态转移模型，其中状态转移矩阵如下,这里演化模型为线性系统，不需要作线性化处理，直接使用即可：
$$F_k=\left[\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$
其中$T$为采样周期。量测信息即为$[{\theta }_{1,t},{\theta }_{2,t}]$,可以看出它是非线性的，根据前面的理论我们可以作线性化处理，这里使用一阶的泰勒级数进行近似，得到$H_{k+1}$矩阵，即为
$$H_{k+1}=\left[\begin{array}{cccc}\left[y_t(k+1)-y_1\right]/r_1& \left[x_t(k+1)-x_1\right]/r_1& 0& 0\\ \left[y_t(k+1)-y_2\right]/r_2& \left[x_t(k+1)-x_2\right]/r_2& 0& 0\end{array}\right]$$
其中
$$\begin{array}{c}{r}_1={\left[y_t(k+1)-y_1\right]}^2+{\left[x_t(k+1)-x_1\right]}^2\\ r_2={\left[y_t(k+1)-y_2\right]}^2+{\left[x_t(k+1)-x_2\right]}^2\end{array}$$
给出python代码如下：

"""
仿真测试二维平面下两个观测站通过纯方位的形式来跟踪一个匀速运动的目标。
已知目标自身的位置，已知目标相对于两个观测站的方位角。
"""
import numpy as np
import math
import matplotlib.pyplot as plt

PI = math.pi

# 设定两个观测点

observer_pos1 = np.array([0, 0])
observer_pos2 = np.array([1000, 0])

# 目标初始位置
init_target_pos = np.array([100, 100])



# 假设目标做匀速直线运动
target_speed = np.array([10, 10])

# 假设每隔1s进行采样，采样10次
T = 1
n = 10

# 状态转移模型使用匀速模型
F = np.array([
    [1, 0, T, 0],
    [0, 1, 0, T],
    [0, 0, 1, 0],
    [0, 0, 0, 1]
    ])

B = np.array(
    [
        [T ** 2/2, 0],
        [0, T ** 2/2],
        [T, 0],
        [0, T],
        ]
)

# 过程噪声矩阵
Q = np.array([
    [1, 0],
    [0, 1],
])

# 量测噪声矩阵
R = np.array([
    [0.1, 0],
    [0, 0.1]
])

# 量测信息为两个方位角，使用泰勒展式进行一阶线性化处理
def getLinearMeasureMat(state, op1, op2):
    """
        state1为状态信息
        op1:观测站1的位置
        op2:观测站2的位置
    """
    r1 = (state[0] - op1[0]) ** 2 + (state[1] - op1[1]) ** 2
    r2 = (state[0] - op2[0]) ** 2 + (state[1] - op2[1]) ** 2

    H = np.array([
        [(op1[1] - state[1]) / r1, (state[0] - op1[0]) / r1, 0, 0],
        [(op2[1] - state[1]) / r2, (state[0] - op2[0]) / r2, 0, 0],
        ])
    return H

# 获取模拟的量测信息
def get_measure_info(target_pos, op1, op2):
    tx = target_pos[0]
    ty = target_pos[1]

    op1_x = op1[0]
    op1_y = op1[1]

    op2_x = op2[0]
    op2_y = op2[1]

    # 计算目标与观测站1的方位角
    # atan的取值范围为(-pi/2,pi/2)
    xita1 = getAngle(tx, ty, op1_x, op1_y)
    xita2 = getAngle(tx, ty, op2_x, op2_y)

    xita1_rad = math.radians(xita1)
    xita2_rad = math.radians(xita2)

    return np.array([xita1_rad, xita2_rad])

def getAngle(tx, ty, opx, opy):
    # x值相同
    if tx == opx:
        if ty > opy:
            return PI/2
        else:
            return -PI/2
    # y值相同
    if ty == opy:
        if tx > opx:
            return 0
        else:
            return PI
        
    tanval = (ty - opy) / (tx - opx)
    angle = math.atan(tanval)
    angle_degree = math.degrees(angle)
    return angle_degree

x0 = np.array([10, 10, 0, 0])
# 初始化协方差矩阵
P0 = np.array([
    [10000, 0, 0, 0],
    [0, 10000, 0, 0],
    [0, 0, 10000, 0],
    [0, 0, 0, 10000]
])

# 存储目标的真实位置信息
target_true_pos = np.empty((n, 2))
# 存储目标的估计位置
target_estimate_pos = np.empty((n, 2))

for i in range(n):
    cur_target_pos = init_target_pos + (i + 1) * T * target_speed
    # print(cur_target_pos)

    # 记录当前的真实目标位置
    target_true_pos[i] = cur_target_pos

    # 一步预测
    x1 = F @ x0
    P1 = F @ P0 @ F.transpose() + B @ Q @ B.transpose()

    # 求当前线性化后的量测矩阵
    Ck = getLinearMeasureMat(x1, observer_pos1, observer_pos2)

    Sk = Ck @ P1 @ Ck.transpose() + R

    # 计算卡尔曼增益
    inv_Sk = np.linalg.inv(Sk)
    K = P1 @ Ck.transpose() @ inv_Sk

    # 获取量测信息
    zk = get_measure_info(cur_target_pos, observer_pos1, observer_pos2)

    # zkk为代入非线性量测方程的实际值
    # zkk = Ck @ x1
    x1_x = x1[0]
    x1_y = x1[1]

    tanv1 = (x1_y - observer_pos1[1]) / (x1_x - observer_pos1[0])
    tanv2 = (x1_y - observer_pos2[1]) / (x1_x - observer_pos2[0])
    apha1 = math.atan(tanv1)
    apha2 = math.atan(tanv2)
    zkk = np.array([apha1, apha2])

    x2 = x1 + K @ (zk - zkk)
    # x2_list = x2.tolist()
    # 从x2中读取真实的预测值
    target_estimate_pos[i] = x2[0:2]

    P2 = P1 - P1 @ K @ Ck

    # 迭代更新
    x0 = x2
    P0 = P2


# 绘图
plt.figure(figsize=(10, 6))
plt.plot(target_true_pos[:, 0], target_true_pos[:, 1], 'b-', label='True Position', linewidth=2)
plt.plot(target_estimate_pos[:, 0], target_estimate_pos[:, 1], 'orange', linestyle='--', label='Estimated Position')
plt.xlabel('X Position')
plt.ylabel('Y Position')
plt.title('True vs Estimated Trajectory')
plt.legend()
plt.grid(True)
plt.axis('equal')
plt.show()

此时可以看到目标的真实位置和EKF滤波器跟踪的结果如下：

可以看到虽然初始的状态向量设置离目标真实状态很大，但随着滤波器不断预测和修正，能够跟踪上目标。注意，由于这里的量测信息使用的是正切值，观测站和目标的x值是不同够一样的。