udacity 无迹卡尔曼滤波（UKF）-python版本(2）

原创 已于 2024-06-15 20:31:51 修改 · 683 阅读 · 11 ·
CC 4.0 BY-SA版权
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
文章标签：

#python #开发语言

于 2024-05-31 10:26:18 首次发布

udacity 无迹卡尔曼滤波（UKF）python版本。根据网上的C++版本改写，方便大家参考。

参考的文章：

（1）https://www.zhihu.com/people/lin-ming-33-56/posts?page=5

感谢作者的分享文章及代码。

一、引用的库

略。

二、定义矩阵类

略。

三、UKF滤波

（1）处理Radar数据

def Ukf(measurement, dt, x, p):
    n_x = 5
    n_aug = 7
    lambda1 = 3 - n_aug
    std_a = 1.6
    std_yawdd = 0.6

    Xsig_aug = matrix([[0] * 15 for _ in range(7)])
    x_aug = matrix([[0] * 1 for _ in range(7)])
    p_aug = matrix([[0] * 7 for _ in range(7)])

    for row in range(n_x):
        x_aug.value[row] = x.value[row]

    for col in range(n_x):
        for row in range(n_x):
            p_aug.value[row][col] = p.value[row][col]

    p_aug.value[5][5] = std_a * std_a
    p_aug.value[6][6] = std_yawdd * std_yawdd

    A = matrix([[0] * 7 for _ in range(7)])
    A = p_aug.Cholesky()
    A = A.transpose()

    for row in range(n_aug):
        Xsig_aug.value[row][0] = x_aug.value[row][0]

    for col in range(n_aug):
        for row in range(n_aug):
            Xsig_aug.value[row][col + 1] = x_aug.value[row][0] + sqrt(3) * A.value[row][col]
            Xsig_aug.value[row][col + 1 + n_aug] = x_aug.value[row][0] - sqrt(3) * A.value[row][col]

    #SigmaPointPrediction()
    Xsig_pred = matrix([[0] * 15 for _ in range(5)])

    delta_t = dt

    for col in range(15):
        p_x = Xsig_aug.value[0][col]
        p_y = Xsig_aug.value[1][col]
        v = Xsig_aug.value[2][col]
        yaw = Xsig_aug.value[3][col]
        yawd = Xsig_aug.value[4][col]
        v_a = Xsig_aug.value[5][col]
        v_b = Xsig_aug.value[6][col]

        if (fabs(yawd) > 0.001):
            px_p = p_x + v / yawd * (sin(yaw + yawd * delta_t) - sin(yaw))
            py_p = p_y + v / yawd * (cos(yaw) - cos(yaw + yawd * delta_t))
        else:
            px_p = p_x + v * delta_t * cos(yaw)
            py_p = p_y + v * delta_t * sin(yaw)

        v_p = v
        yaw_p = yaw + yawd * delta_t
        yawd_p = yawd

        px_p = px_p + 0.5 * v_a * delta_t * delta_t * cos(yaw)
        py_p = py_p + 0.5 * v_a * delta_t * delta_t * sin(yaw)
        v_p = v_p + v_a * delta_t
        yaw_p = yaw_p + 0.5 * v_b * delta_t * delta_t
        yawd_p = yawd_p + v_b * delta_t

        Xsig_pred.value[0][col] = px_p
        Xsig_pred.value[1][col] = py_p
        Xsig_pred.value[2][col] = v_p
        Xsig_pred.value[3][col] = yaw_p
        Xsig_pred.value[4][col] = yawd_p

    #PredictMeanAndCovariance()
    weights = matrix([[0] * 15 for _ in range(1)])
    x = matrix([[0] * 1 for _ in range(5)])
    p = matrix([[0] * 5 for _ in range(5)])
    x_diff = matrix([[0] * 1 for _ in range(5)])
    x_diff_w = matrix([[0] * 1 for _ in range(5)])

    weights.value[0][0] = lambda1 / (lambda1 + n_aug)
    weight = 0.5 / (lambda1 + n_aug)
    for col in range(1, 15):
        weights.value[0][col] = weight

    for col in range(15):
        for row in range(5):
            x.value[row][0] = x.value[row][0] + weights.value[0][col] * Xsig_pred.value[row][col]

    M_PI = 3.1415926
    for col in range(15):
        for row in range(5):
            x_diff.value[row][0] = Xsig_pred.value[row][col] - x.value[row][0]

            while (row == 3 and x_diff.value[row][0] > M_PI):
                x_diff.value[row][0] = x_diff.value[row][0] - 2 * M_PI
            while (row == 3 and x_diff.value[row][0] < -M_PI):
                x_diff.value[row][0] = x_diff.value[row][0] + 2 * M_PI

            x_diff_w.value[row][0] = weights.value[0][col] * x_diff.value[row][0]

        p = p + x_diff_w.__mul__(x_diff.transpose())

    #PredictRadarMeasurement():
    n_z = 3

    std_radr = 0.3
    std_radphi = 0.03
    std_radrd = 0.3

    z_diff = matrix([[0] * 1 for _ in range(3)])
    z_diff_w = matrix([[0] * 1 for _ in range(3)])

    Zsig = matrix([[0] * 15 for _ in range(3)])
    z_pred = matrix([[0] * 1 for _ in range(3)])
    S = matrix([[0] * 3 for _ in range(3)])
    R = matrix([[0] * 3 for _ in range(3)])

    for col in range(15):
        p_x = Xsig_pred.value[0][col]
        p_y = Xsig_pred.value[1][col]
        v = Xsig_pred.value[2][col]
        yaw = Xsig_pred.value[3][col]
        yawd = Xsig_pred.value[4][col]

        th_2 = sqrt(p_x * p_x + p_y * p_y)
        rho_z = th_2
        yaw_z = atan2(p_y, p_x)
        rhod_z = (p_x * cos(yaw) * v + p_y * sin(yaw) * v) / th_2

        Zsig.value[0][col] = rho_z
        Zsig.value[1][col] = yaw_z
        Zsig.value[2][col] = rhod_z

    for col in range(15):
        for row in range(3):
            z_pred.value[row][0] = z_pred.value[row][0] + weights.value[0][col] * Zsig.value[row][col]

    for col in range(15):
        for row in range(3):
            z_diff.value[row][0] = Zsig.value[row][col] - z_pred.value[row][0]

            while (row == 1 and z_diff.value[row][0] > M_PI):
                z_diff.value[row][0] = z_diff.value[row][0] - 2 * M_PI
            while (row == 1 and z_diff.value[row][0] < -M_PI):
                z_diff.value[row][0] = z_diff.value[row][0] + 2 * M_PI

            z_diff_w.value[row][0] = weights.value[0][col] * z_diff.value[row][0]

        S = S + z_diff_w.__mul__(z_diff.transpose())

    R = matrix([[std_radr * std_radr, 0, 0], [0, std_radphi * std_radphi, 0], [0, 0, std_radrd * std_radrd]])
    S = S + R

    #UpdateState():
    Tc = matrix([[0] * n_z for _ in range(n_x)])
    K = matrix([[3] * 1 for _ in range(5)])

    z = measurement

    for col in range(15):
        for row1 in range(5):
            x_diff.value[row1][0] = Xsig_pred.value[row1][col] - x.value[row1][0]

            while (row1 == 3 and x_diff.value[row1][0] > M_PI):
                x_diff.value[row1][0] = x_diff.value[row1][0] - 2 * M_PI
            while (row1 == 3 and x_diff.value[row1][0] < -M_PI):
                x_diff.value[row1][0] = x_diff.value[row1][0] + 2 * M_PI

            x_diff_w.value[row1][0] = weights.value[0][col] * x_diff.value[row1][0]

        for row2 in range(3):
            z_diff.value[row2][0] = Zsig.value[row2][col] - z_pred.value[row2][0]

            while (row2 == 1 and z_diff.value[row2][0] > M_PI):
                z_diff.value[row2][0] = z_diff.value[row2][0] - 2 * M_PI
            while (row2 == 1 and z_diff.value[row2][0] < -M_PI):
                z_diff.value[row2][0] = z_diff.value[row2][0] + 2 * M_PI

        Tc = Tc + x_diff_w.__mul__(z_diff.transpose())

    K = Tc.__mul__(S.inverse())

    z_diff = z - z_pred
    while (z_diff.value[1][0] > M_PI):
        z_diff.value[1][0] = z_diff.value[1][0] - 2 * M_PI
    while (z_diff.value[1][0] < -M_PI):
        z_diff.value[1][0] = z_diff.value[1][0] + 2 * M_PI

    x = x + K.__mul__(z_diff)
    KS = K.__mul__(S)
    p = p - KS.__mul__(K.transpose())
    return [x, p]

#end ukf()

（2）处理Lidar数据

    #PredictRadarMeasurement():
    n_z = 2

    std_laspx_ = 0.15
    std_laspy_ = 0.15

    z_diff = matrix([[0] * 1 for _ in range(2)])
    z_diff_w = matrix([[0] * 1 for _ in range(2)])

    Zsig = matrix([[0] * 15 for _ in range(2)])
    z_pred = matrix([[0] * 1 for _ in range(2)])
    S = matrix([[0] * 2 for _ in range(2)])
    R = matrix([[0] * 2 for _ in range(2)])

    for col in range(15):
        p_x = Xsig_pred.value[0][col]
        p_y = Xsig_pred.value[1][col]

        Zsig.value[0][col] = p_x
        Zsig.value[1][col] = p_y

    for col in range(15):
        for row in range(2):
            z_pred.value[row][0] = z_pred.value[row][0] + weights.value[0][col] * Zsig.value[row][col]

    for col in range(15):
        for row in range(2):
            z_diff.value[row][0] = Zsig.value[row][col] - z_pred.value[row][0]
            z_diff_w.value[row][0] = weights.value[0][col] * z_diff.value[row][0]

        S = S + z_diff_w.__mul__(z_diff.transpose())

    R = matrix([[std_laspx_ * std_laspx_, 0], [0, std_laspy_ * std_laspy_]])
    S = S + R

    #UpdateState():
    Tc = matrix([[0] * n_z for _ in range(n_x)])
    K = matrix([[2] * 1 for _ in range(5)])

    z = measurement

    for col in range(15):
        for row1 in range(5):
            x_diff.value[row1][0] = Xsig_pred.value[row1][col] - x.value[row1][0]
            x_diff_w.value[row1][0] = weights.value[0][col] * x_diff.value[row1][0]

        for row2 in range(2):
            z_diff.value[row2][0] = Zsig.value[row2][col] - z_pred.value[row2][0]

        Tc = Tc + x_diff_w.__mul__(z_diff.transpose())

    K = Tc.__mul__(S.inverse())

    z_diff = z - z_pred

    x = x + K.__mul__(z_diff)
    KS = K.__mul__(S)
    p = p - KS.__mul__(K.transpose())
    return [x, p]

#end ukf()

四、数据处理

（1）处理Radar数据

file1 = open('obj_pose-laser-radar-synthetic-input.txt', 'r')
file2 = open('obj_pose-laser-radar-synthetic-output-radar-ukf.txt', 'w+')

#input file format:
#L(for laser) meas_px meas_py timestamp gt_px gt_py gt_vx gt_vy
#R(for radar) meas_rho meas_phi meas_rho_dot timestamp gt_px gt_py gt_vx gt_vy
#Output file format:
#est_px est_py est_v gt_px gt_py

meas = matrix([[0] * 1 for _ in range(3)]) # initial measurement
x = matrix([[0] * 1 for _ in range(5)]) # state
p = matrix([[0] * 5 for _ in range(5)]) # covariance
b_init = 0

data = file1.readlines()
for line in data:
    line = line.split('\t')
    sensor = line[0]
    if(sensor == 'R'):
        meas.value[0][0] = float(line[1]) #rho
        meas.value[1][0] = float(line[2]) #phi
        meas.value[2][0] = float(line[3]) #rho-dot

        time = float(line[4])
        gt_px = float(line[5])
        gt_py = float(line[6])

        if(b_init == 0):
            b_init = 1
            pre_t = time

            px = meas.value[0][0] * cos(meas.value[1][0])
            py = meas.value[0][0] * sin(meas.value[1][0])
            vx = meas.value[2][0] * cos(meas.value[1][0])
            vy = meas.value[2][0] * sin(meas.value[1][0])

            x.value[0][0] = px
            x.value[1][0] = py
            x.value[2][0] = 0.
            x.value[3][0] = 0.
            x.value[4][0] = 0.

            p = matrix([[1, 0, 0, 0, 0],
                        [0, 1, 0, 0, 0],
                        [0, 0, 1, 0, 0],
                        [0, 0, 0, 1, 0],
                        [0, 0, 0, 0, 1]])

            file2.write(str(x.value[0][0]) + ' ' + #px
                        str(x.value[1][0]) + ' ' + #py
                        str(x.value[2][0]) + ' ' + #v
                        str(gt_px) + ' ' +         #gt_px
                        str(gt_py) +'\n')          #gt_py
        else:
            dt = (time - pre_t) / 1000000.0
            pre_t = time

            [x, p] = Ukf(meas, dt, x, p)
            file2.write(str(x.value[0][0]) + ' ' +  # px
                        str(x.value[1][0]) + ' ' +  # py
                        str(x.value[2][0]) + ' ' +  # v
                        str(gt_px) + ' ' +          # gt_px
                        str(gt_py) + '\n')          # gt_py
file1.close()
file2.close()

（2）处理Lidar数据

#main
file1 = open('obj_pose-laser-radar-synthetic-input.txt', 'r')
file2 = open('obj_pose-laser-radar-synthetic-output-lidar-ukf.txt', 'w+')

#input file format:
#L(for laser) meas_px meas_py timestamp gt_px gt_py gt_vx gt_vy
#R(for radar) meas_rho meas_phi meas_rho_dot timestamp gt_px gt_py gt_vx gt_vy
#Output file format:
#est_px est_py  gt_px gt_py

meas = matrix([[0] * 1 for _ in range(2)]) # initial measurement
x = matrix([[0] * 1 for _ in range(5)]) # state
p = matrix([[0] * 5 for _ in range(5)]) # covariance
b_init = 0

data = file1.readlines()
for line in data:
    line = line.split('\t')
    sensor = line[0]
    if(sensor == 'L'):
        meas.value[0][0] = float(line[1]) #meas_px
        meas.value[1][0] = float(line[2]) #meas_py

        time = float(line[3])
        gt_px = float(line[4])
        gt_py = float(line[5])

        if(b_init == 0):
            b_init = 1
            pre_t = time

            px = meas.value[0][0]
            py = meas.value[1][0]

            x.value[0][0] = px
            x.value[1][0] = py

            p = matrix([[1, 0, 0, 0, 0],
                        [0, 1, 0, 0, 0],
                        [0, 0, 1, 0, 0],
                        [0, 0, 0, 1, 0],
                        [0, 0, 0, 0, 1]])

            file2.write(str(x.value[0][0]) + ' ' + #px
                        str(x.value[1][0]) + ' ' + #py
                        str(gt_px) + ' ' +         #gt_px
                        str(gt_py) +'\n')          #gt_py
        else:
            dt = (time - pre_t) / 1000000.0
            pre_t = time

            [x, p] = Ukf(meas, dt, x, p)
            file2.write(str(x.value[0][0]) + ' ' +  # px
                        str(x.value[1][0]) + ' ' +  # py
                        str(gt_px) + ' ' +          # gt_px
                        str(gt_py) + '\n')          # gt_py
file1.close()
file2.close()

五、结果对比

（1）对于Radar数据，将ukf滤波获取的结果与真值以及ekf获取的结果进行对比，如下图示：

基本上ukf、ekf滤波结果差别不大。

（2）ukf滤波分别对Radar数据与Lidar数据处理，结果如下图示：

从图上看，同样是Ukf滤波，Lidar传感器的效果好于Radar传感器。

（3）Lidar数据，分别用Ukf滤波以及KF滤波进行处理，结果如下图示：

从图上看，对于Lidar传感器数据，采用ukf滤波与KF滤波效果差别不大。

六、关于cholesky分解

本程序中使用的cholesky分解函数对于某些矩阵会判定为非正定矩阵，报错；

本程序中尝试采用np.linalg.cholesky()进行分解，同样报错；

而c++程序中P.llt().matrixL()进行分解，却没有报错。

有知道原因的网友解释一下，谢谢。

capitel2
关注
滤波算法 | 无迹卡尔曼滤波（UKF）算法及其Python实现
lovetaozibaby的博客
03-10 7998
上一篇文章，我们介绍了UKF滤波公式及其MATLAB代码。在做视觉测量的过程中，基于OpenCV的开发包比较多，因此我们将UKF的MATLAB代码转到python中，实现数据滤波效果。UKF滤波公式及其MATLAB代码这里简单把上一篇文章的公式和流程图粘贴一下。
滤波笔记三：无迹卡尔曼滤波（UKF）
scoutee的博客
09-08 1万+
UKF
ukf.py 无迹卡尔曼滤波 python demo
08-03
python实现的无迹卡尔曼滤波算法，ukf 1 ~3 的观测方程不一样，感觉观测方程是平方时 有点莫名其妙。有兴趣的朋友下载自行研究。
UKF（Unscented Kalman Filter) 无迹卡尔曼滤波程序代码
06-05
本代码实现是对UKF（无迹卡尔曼滤波）的MATLAB实现
无迹卡尔曼滤波-1
weixin_34146410的博客
03-02 974
无迹卡尔曼滤波(unscented kalman filter)中需要用到无迹变换。维基百科中对unscented transform的描述如下： The unscented transform (UT) is a mathematical function used to estimate the result of applying a given nonlinear transformat...
UKF算法的代码
03-19
UKF的matlab代码
udacity 扩展卡尔曼滤波（EKF）-python版本
capitel2的博客
04-26 697
本程序用于处理毫米波雷达数据。参考udacity资料以及网络资料。
无迹（损）卡尔曼滤波（UKF）理论讲解与实例
热门推荐
O_MMMM_O的博客
05-15 3万+
无迹（损）卡尔曼滤波（EKF）理论讲解与实例 文章目录无迹（损）卡尔曼滤波（EKF）理论讲解与实例理论讲解模型对比UT变换UKF算法步骤预测部分更新部分应用实例CTRV模型预测处理产生点云生成增广矩阵生成预测点计算预测的均值和方差更新处理预测量测值计算预测量测值的均值和方差更新状态完整代码参考链接 理论讲解 前两篇博客的卡尔曼滤波和扩展卡尔曼滤波都是都将问题转化为线性高斯模型，所以可以直接解出贝叶斯递推公式中的解析形式，方便运算。但对于非线性问题， 扩展卡尔曼滤波除了计算量大，还有线性误差的影响，有没有别的
从贝叶斯滤波到无迹卡尔曼滤波
ShiPeng
01-12 1362
目录标题0 引言1 无迹变换1.1 什么是无迹变换1.2 一般形式的无迹变换1.3 比例无迹变换2 无迹卡尔曼滤波的假设2.1 与卡尔曼滤波相同的假设2.2 与卡尔曼滤波不同的假设3 无迹卡尔曼滤波算法框架3.1 可加性噪声条件下的无迹卡尔曼滤波3.2 非可加性噪声条件下的无迹卡尔曼滤波4 应用实例——基于毫米波雷达与无迹卡尔曼滤波的目标跟踪4.1 系统分析4.1.1 状态转移过程分析4.1.2 观测过程分析4.2 代码实现5 总结参考 0 引言 在此前的文章《从贝叶斯滤波到扩展卡尔曼滤波》中，我们讲述了扩
python卡尔曼滤波融合_数据融合之卡尔曼滤波示例
weixin_28849929的博客
01-11 2834
哎，难受，知乎把我的完美格式打乱成这样子。卡尔曼几乎是做数据融合都知道的一个算法了把。卡尔曼滤波能够实现从有噪声的传感器数据中获取我们的较为准确的信息。而且通过卡尔曼，我们甚至能够去推算一些不可直接测量的值。1.高斯知识的简单回顾在卡尔曼滤波器中，我们的任务就是估计出物体最佳的期望以及方差出来。接下来我们来些一个简单的高斯函数def f(mu,sigma,x):return 1/sqrt(2.*p...
UKF无迹卡尔曼滤波器代码实现
03-31
以匀速直线运动为例，设计了基于距离的目标跟踪算法，即状态量为X、Y轴的位置和速度，观测值为物体到观测站的距离，具体实现过程见代码
无迹卡尔曼滤波代码
01-26
首先你需要知道卡尔曼滤波，卡尔曼滤波适用于线性系统，针对于非线性系统很好推广应用。EKF利用线性化的方式，让状态和协方差在线性化方程中传播，但是面对强非线性，这种方式误差较大，因为高斯分布的噪声经过非线性系统的分布并不是高斯分布。UKF利用5个采样点（无迹变换）在非线性系统中传播，降低了随机变量经过非线性系统传播的误差，效果强于EKF。针对P矩阵出现非正定的情况，其实有很多处理方式的。
本人自己写的UKF代码
06-08
无损卡尔曼滤波器
UKF的C、C++代码实现
12-09
老外写的一些UKF相关的C＋＋代码 对需要UKF编程的人有帮助, 学习卡尔曼滤波技术的最佳材料，易懂易看利于快速进行二次开发
卡尔曼滤波Python代码实例实现
05-08
卡尔曼滤波算法(KF)是序贯数据同化的一种，是由Kalman针对随机过程状态估计提出的。KF的基本思想是利用前一时刻的状态估计值和当前时刻的观测值来获得动态系统当前时刻状态变量的最优估计。
python 无迹卡尔曼（filterpy.kalman.UnscentedKalmanFilter）调参
weixin_46180132的博客
05-20 5329
python 无迹卡尔曼（filterpy.kalman）调参 1. filter.kalman.MerweScaledSigmaPoints，Van der Merwe式sigma点选择，UT变换最好用的sigma点选择算法 input： n ：状态维度 alpha ：均值附近sigma点的分布，通常选0.1~1之间。调节中心点与周围点的权重。 beta ：由先验知识，高斯分布选2。 kappa ：k = 3-n output: Wm ：sigma点均值的权重 Wc ：sigma点协方差的权重 可以看到，
kf，ukf，ekf学习及应用python
lily_abcd的博客
06-06 681
UKF算法通过选取 sigma 点，将系统的非线性映射转化为一组线性运算，然后利用卡尔曼滤波的思想进行状态估计。相比于传统的线性卡尔曼滤波，UKF算法可以有效地处理非线性系统，提供更加准确的状态估计结果。中使用卡尔曼滤波是非常理想的，它具有占用内存小的优点（除了前一个状态量外，不需要保留其它历史数据），并且速度很快，很适合应用于实时问题和嵌入式系统。想系统的学习一下卡尔曼滤波的内容，学习初衷是为了进行传感器的融合实验，主要是进行去向的融合实验。的动态系统中使用卡尔曼滤波，对系统下一步的走向做出。
用python编写的UKF（无迹卡尔曼滤波）代码，状态量和观测量都是三维、非线性的，注释使用中文
985工科博士毕业，专攻定位、导航和滤波等算法研究。从业10年，主要使用MATLAB
01-18 1205
无迹卡尔曼滤波器适用于处理非线性系统，通过生成和加权 sigma 点来有效捕捉状态的分布信息。这段代码提供了一个基本的 UKF 实现，涵盖了状态预测、观测更新和协方差更新的完整流程。如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者。
卡尔曼滤波的Python实现
乐观的lishan的博客
05-04 2万+
本文实现了卡尔曼滤波的python代码
扩展卡尔曼滤波C++
最新发布
06-18
### 扩展卡尔曼滤波 C++ 实现教程与代码示例 扩展卡尔曼滤波（Extended Kalman Filter, EKF）是一种针对非线性系统的状态估计方法，其核心思想是通过线性化非线性系统模型来近似应用标准卡尔曼滤波算法。在 C++ 中实现 EKF 时，可以利用类和函数来组织代码，从而提高可读性和可维护性。 以下是一个完整的 C++ 实现 EKF 的示例代码，涵盖了状态预测、协方差预测、状态更新和协方差更新等基本步骤[^1]。 ```cpp #include <iostream> #include <vector> #include <Eigen/Dense> // 使用 Eigen 库处理矩阵运算 using namespace std; using namespace Eigen; class ExtendedKalmanFilter { public: ExtendedKalmanFilter(VectorXd x_in, MatrixXd P_in, MatrixXd F_in, MatrixXd H_in, MatrixXd R_in, MatrixXd Q_in) : x(x_in), P(P_in), F(F_in), H(H_in), R(R_in), Q(Q_in) {} void Predict() { x = F * x; // 状态预测 P = F * P * F.transpose() + Q; // 协方差预测 } void Update(const VectorXd &z) { VectorXd y = z - H * x; // 测量残差 MatrixXd S = H * P * H.transpose() + R; // 残差协方差 MatrixXd K = P * H.transpose() * S.inverse(); // 卡尔曼增益 x = x + K * y; // 状态更新 P = (MatrixXd::Identity(x.size(), x.size()) - K * H) * P; // 协方差更新 } private: VectorXd x; // 状态向量 MatrixXd P; // 状态协方差矩阵 MatrixXd F; // 状态转移矩阵 MatrixXd H; // 观测矩阵 MatrixXd R; // 测量噪声协方差矩阵 MatrixXd Q; // 过程噪声协方差矩阵 }; int main() { // 初始化参数 VectorXd x = VectorXd(2); // 初始状态 [位置, 速度] x << 0, 0; MatrixXd P = MatrixXd(2, 2); // 初始协方差矩阵 P << 1, 0, 0, 1; MatrixXd F = MatrixXd(2, 2); // 状态转移矩阵 F << 1, 1, 0, 1; MatrixXd H = MatrixXd(1, 2); // 观测矩阵 H << 1, 0; MatrixXd R = MatrixXd(1, 1); // 测量噪声协方差矩阵 R << 1; MatrixXd Q = MatrixXd(2, 2); // 过程噪声协方差矩阵 Q << 0.1, 0, 0, 0.1; // 创建 EKF 对象 ExtendedKalmanFilter ekf(x, P, F, H, R, Q); // 模拟测量数据 VectorXd z = VectorXd(1); z << 1.0; // 假设测量值为 1.0 // 预测与更新 ekf.Predict(); ekf.Update(z); // 输出结果 cout << "Updated state: " << ekf.x.transpose() << endl; cout << "Updated covariance: " << endl << ekf.P << endl; return 0; } ``` 上述代码中使用了 **Eigen** 库来处理矩阵运算，这是一个高效的开源线性代数库。为了运行此代码，请确保安装了 Eigen 库，并在编译时链接相应的头文件。 #### 关键点解析 - **状态预测**：通过状态转移矩阵 `F` 和当前状态 `x` 计算下一时刻的状态。 - **协方差预测**：通过过程噪声协方差矩阵 `Q` 更新状态协方差矩阵 `P`。 - **状态更新**：根据测量值 `z` 和观测矩阵 `H` 计算测量残差 `y`。 - **协方差更新**：通过卡尔曼增益 `K` 调整状态协方差矩阵 `P`。 此外，还可以参考 Udacity 提供的 Python 版本实现[^2]，以及 GitHub 上的项目 `JunshengFu/tracking-with-Extended-Kalman-Filter`，这些资源可以帮助进一步理解 EKF 的原理和应用。 ### 注意事项 在实际应用中，EKF 的性能依赖于对系统模型和噪声特性的准确建模。如果非线性程度较高或噪声特性复杂，可能需要考虑其他滤波方法，如无迹卡尔曼滤波（UKF）或粒子滤波（PF）[^3]。
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
表情包 代码片
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值