Python 实现卡尔曼滤波器 --非常简单

最新推荐文章于 2024-08-07 11:08:25 发布

三川小哥知乎

最新推荐文章于 2024-08-07 11:08:25 发布

阅读量8.2k

点赞数 5

分类专栏： ROS

本文链接：https://blog.csdn.net/qq_27262241/article/details/85089900

版权

本文介绍了如何使用Python实现卡尔曼滤波器，通过结合GPS和里程计数据进行数据融合，以提高定位精度。

摘要由CSDN通过智能技术生成

整体思路很简单，卡尔曼滤波器就是做数据融合的，先给一个GPS的数据（z）和一个里程计数据（u），让他们融合吧。

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Tue Dec 18 19:37:13 2018

@author: sc
args explanition:
    p:covariance
    x:state
    z:observation
    u:control
    Pred:predict
    Est:estatemation
    Q：表示过程激励噪声的协方差，它是状态转移矩阵与实际过程之间的误差。
    R：表示测量噪声协方差，和仪器相关的一个特性，
"""
import numpy as np
import math 
import matplotlib.pyplot as plt
Q=np.diag([0.1,0.1,math.radians(1.0),1.0])**2
R=np.diag([1.0,math.radians(40.0)])**2
dt=0.1
def motion_model(x,u):
    B=np.matrix([[dt*math.cos(x[2,0]),0.0],
                  [dt*math.sin(x[2,0]),0.0],
                  [0.0,dt],
                  [1.0,0.0]])
    x=x+B*u
    return x
#def observe_model(z):
#    H=
#    pass
#def JacoMo(xEst,u):
#    return jMo
#def JacoOb(xEst):
#    return