卡尔曼滤波预测轨迹python 卡尔曼滤波预测轨迹

 

1. Kalman filter基本介绍

卡尔曼滤波（Kalman filter）是一种高效的自回归滤波器，它能在存在诸多不确定性情况的组合信息中估计动态系统的状态，是一种强大的、通用性极强的工具。通俗一点来讲就是通过一系列不那么准确的观测值来预测真实值。

图1. 一个简单的kalman filter应用

在上图中红线real是真实的运动状态，绿线measure是测量值，蓝线filter是卡尔曼滤波预测的结果。我们可以认为这里的横轴表示时间，纵轴表示位置或速度。
在一个真实的运动系统中，仅有观测值是可知的，且观测值相对于真实值会存在误差，我们认为观测值在真实值附近呈现高斯分布（Gaussian distribution）。那么我们就可以使用卡尔曼滤波进行一个合理的预测了。
简而言之，Kalman filter就是一个用来预测的工具，并且它是通用的、高效的！

2. Kalman filter的核心思路

图2. kalman filter的预测与更新

卡尔曼滤波通过“预测”与“更新”两个过程来对系统的状态进行最优估计。在使用代码的编辑过程中，我们通常像下面这样来编写KalmanFilter类。

# 以python代码为示例,请暂时忽略传入的参数
class KalmanFilter(object):
	def __init__(self, F = None, B = None, H = None, Q = None, R = None, P = None, x0 = None):
	def predict(self, u = 0):
	def update(self, z):

使用卡尔曼滤波进行预测就是按照图2的思路进行函数的调用。大致如下：

#以下代码仅为思路示例
for z in measurements:
	kf.predict()
	kf.update()

3. Kalman filter的相应公式

图2. kalman filter公式

相应公式的详细理解可以参考：【工程师学算法】工程常用算法（二）—— 卡尔曼滤波(Kalman Filter)、卡尔曼滤波（Kalman Filter）原理与公式推导。

4. Kalman filter详细代码

摘抄自：https://zhuanlan.zhihu.com/p/113685503

import numpy as np

class KalmanFilter(object):
    def __init__(self, F = None, B = None, H = None, Q = None, R = None, P = None, x0 = None):

        if(F is None or H is None):
            raise ValueError("Set proper system dynamics.")

        self.n = F.shape[1]
        self.m = H.shape[1]

        self.F = F
        self.H = H
        self.B = 0 if B is None else B
        self.Q = np.eye(self.n) if Q is None else Q
        self.R = np.eye(self.n) if R is None else R
        self.P = np.eye(self.n) if P is None else P
        self.x = np.zeros((self.n, 1)) if x0 is None else x0

    def predict(self, u = 0):
        self.x = np.dot(self.F, self.x) + np.dot(self.B, u)
        self.P = np.dot(np.dot(self.F, self.P), self.F.T) + self.Q
        return self.x

    def update(self, z):
        y = z - np.dot(self.H, self.x)
        S = self.R + np.dot(self.H, np.dot(self.P, self.H.T))
        K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))
        self.x = self.x + np.dot(K, y)
        I = np.eye(self.n)
        self.P = np.dot(I - np.dot(K, self.H), self.P)

5. 卡尔曼滤波推荐阅读资料：

基础入门类：
https://classroom.udacity.com/courses/cs373/https://zhuanlan.zhihu.com/p/39912633

https://zhuanlan.zhihu.com/p/48876718

代码示例类：

https://zhuanlan.zhihu.com/p/113685503