kf，ukf，ekf学习及应用python

想系统的学习一下卡尔曼滤波的内容，学习初衷是为了进行传感器的融合实验，主要是进行去向的融合实验。

什么是卡尔曼滤波（kf）？（只给出简要的介绍）

        在任何含有不确定信息的动态系统中使用卡尔曼滤波，对系统下一步的走向做出有根据的预测，即使伴随着各种干扰，卡尔曼滤波总是能指出真实发生的情况。在连续变化的系统中使用卡尔曼滤波是非常理想的，它具有占用内存小的优点（除了前一个状态量外，不需要保留其它历史数据），并且速度很快，很适合应用于实时问题和嵌入式系统。

具体的原理参考https://blog.csdn.net/seek97/article/details/120012667

什么是无迹卡尔曼滤波？（ukf）

        ukf算法是一种非线性滤波算法，能够有效地处理复杂非线性系统的状态估计问题。其核心思想是通过一组选定的 sigma 点来逼近系统运动的高斯分布，从而实现状态估计。UKF算法通过选取 sigma 点，将系统的非线性映射转化为一组线性运算，然后利用卡尔曼滤波的思想进行状态估计。相比于传统的线性卡尔曼滤波，UKF算法可以有效地处理非线性系统，提供更加准确的状态估计结果。

ukf算法公式推导请参考超详细讲解无迹卡尔曼（UKF）滤波（个人整理结合代码分析）_无迹卡尔曼滤波-CSDN博客https://blog.csdn.net/jiushizhemekeai/article/details/127453800

本文给出利用ukf进行曲线融合的python代码，代码已经进行了参数的调优

def fx(x, dt):
    return np.array([x[0] + x[2]*dt*dt, x[1] + x[3]*dt*dt, x[2]*dt*dt, x[3]*dt*dt])

def hx(x):
    return np.array([x[0], x[1]])

# 初始化UKF
def initialize_ukf(dt=0.9):
    # 创建Sigma点，参数定义如下：
    # n=4: 状态向量的维度
    # alpha=0.1: Sigma点分布的缩放参数，控制分布的广度
    # beta=2.0: 用于捕获先验分布的尾部重量，beta=2是最佳选择对于高斯分布
    # kappa=0: 二次项的缩放参数，影响Sigma点分布
    points = MerweScaledSigmaPoints(n=4, alpha=0.01, beta=2., kappa=0)
    
    # 初始化UKF
    # dim_x=4: 状态空间的维度
    # dim_z=2: 观测空间的维度
    # dt=dt: 时间步长
    # fx=fx: 状态转移函数，定义状态如何随时间演进
    # hx=hx: 观测函数，定义如何从状态变量产生观测数据
    # points=points: 使用上面定义的Sigma点
    ukf = UnscentedKalmanFilter(dim_x=4, dim_z=2, dt=dt, fx=fx, hx=hx, points=points)
    
    # 设置初始状态向量，全部初始化为0
    ukf.x = np.array([0., 0., 0., 0.])
    
    # 初始化协方差矩阵
    ukf.P *= 10.
    
    # 设置观测噪声矩阵
    ukf.R = np.diag([1, 1])
    
    # 设置过程噪声矩阵
    ukf.Q = np.eye(4) * 0.0001
    
    # 返回初始化完成的UKF对象
    return ukf

ekf算法Python的执行过程参考如下

#单传感器，线性系统
def uss_KF_fusion(uss_measurements):
    '''
    一、单传感器情况，假设是线性系统
    
    1.预测方程：
    X(K) = X(K-1) + w
    其中，w是控制系统的噪声
    
    2.观测方程
    Y(K) = H * X + v
    其中，H=[1],v是测量噪声 
    '''
    A = np.mat([1])
    H = np.mat([1])
    I = np.mat([1])

    P = np.mat([0.0001])
    Q = np.mat([0.1])
    R = np.mat([0.01])

    X_hat_k_1 = np.mat([0.1])
    P_k_1 = P
    
    fusion_output = np.array([])
    
    for i in range(length): 
        X_hat_k_ = A * X_hat_k_1
        P_k_ = A * P_k_1 * A.T + Q
        
        K = P_k_ * H.T / (H * P_k_ *H.T + R)
        X_hat_k = X_hat_k_ + K * (uss_measurements[i] - H * X_hat_k_)
        P_k = (I - K * H) * P_k_
        
        fusion_output = np.append(fusion_output, X_hat_k[0])
        
        X_hat_k_1 = X_hat_k
        P_k_1 = P_k

    return fusion_output

#单传感器，非线性系统
def uss_EKF_fusion(uss_measurements):
    '''
    二、单传感器情况，假设系统是非线性的
    预测方程不再用线性方程表示，而是在k-1时刻展开为泰勒级数，来近似表示k时刻的状态：
    X(K) = X(K-1)/0! + X'(K-1)*dt/1! + X''(K-1)*dt^2/2! + Q
    其中,dt表示当前时刻与上一时刻的时间间隔
    此时，状态变量变为：X = [X(K), X'(K), X''(K)].T
    
    1.此时预测方程为：
      X(K) = X(K-1)/0! + X'(K-1)*dt/1! +  X''(K-1)*dt^2/2! + w1
      X'(K) =             X'(K-1)/0!   +  X''(K-1)*dt/1! + w2
      X''(K) =                            X''(K-1)/0! + w3
      
      其中，w是控制系统的噪声
      
    2.那么状态转移矩阵为：
          1 dt  dt^2/2!
      A = 0 1   dt
          0 0   1
      
    3.那么观测方程为：
      Y(K) = H * X + v, 其中，H = [1, 0, 0]，v是测量的噪声
    '''
     
    dt = 0.01
    base = 2
    
    A = np.mat([[1, dt, dt**2*0.5],
                  [0, 1, dt],
                  [0, 0, 1]])
    H = np.mat([1, 0, 0])
    I = np.eye(3)

    P = np.mat([[1, 0, 0],
                [0, base * 0.01, 0],
                [0, 0, base * 0.01 * 0.01]])
    Q = np.mat([[1, 0, 0],
                [0, base * 0.01, 0],
                [0, 0, base * 0.01 * 0.01]])
    R = np.mat([10])
        
    X_hat_k_1 = np.mat([0.01, 0, 0]).T
    P_k_1 = P
    
    fusion_output = np.array([])
    
    for i in range(length): 
        X_hat_k_ = A * X_hat_k_1
        P_k_ = A * P_k_1 * A.T + Q
        
        K = P_k_ * H.T / (H * P_k_ *H.T + R)
        X_hat_k = X_hat_k_ + K * (uss_measurements[i] - H * X_hat_k_)
        P_k = (I - K * H) * P_k_
        
        fusion_output = np.append(fusion_output, X_hat_k[0])
        
        X_hat_k_1 = X_hat_k
        P_k_1 = P_k
        
    return fusion_output

#多传感器，非线性系统
def uss_video_EKF_fusion(uss_measurements, video_measurements):
    '''
    三、多传感器情况，假设系统是非线性的
    状态向量X不变，状态转移矩阵A不变，系统噪声协方差矩阵Q不变
    
    观测矩阵H要变，观测噪声协方差矩阵R要变（观测数据维度变了）
    '''
    dt = 0.01
    base = 1
    
    A = np.mat([[1, dt, dt**2*0.5],
                  [0, 1, dt],
                  [0, 0, 1]])
    H = np.mat([[1, 0, 0],
                [1, 0, 0]])
    I = np.eye(3)

    P = np.mat([[1, 0, 0],
                [0, base * 0.01, 0],
                [0, 0, base * 0.01 * 0.01]])
    Q = np.mat([[1, 0, 0],
                [0, base * 0.01, 0],
                [0, 0, base * 0.01 * 0.01]])
    R = np.mat([[10, 0],
                [0, 10]])
        
    X_hat_k_1 = np.mat([0.01, 0, 0]).T
    P_k_1 = P
    
    measurements = np.vstack((uss_measurements, video_measurements))
    fusion_output = np.array([])
    
    for i in range(length): 
        X_hat_k_ = A * X_hat_k_1
        P_k_ = A * P_k_1 * A.T + Q
        
        K = P_k_ * H.T * np.linalg.pinv(H * P_k_ *H.T + R)
        X_hat_k = X_hat_k_ + K * (measurements[:,i].reshape(2, 1) - H * X_hat_k_)
        P_k = (I - K * H) * P_k_
        
        fusion_output = np.append(fusion_output, X_hat_k[0])
        
        X_hat_k_1 = X_hat_k
        P_k_1 = P_k
    
    return fusion_output