卡尔曼滤波理论讲解与应用（matlab和python）

理论讲解

使用前提

卡尔曼滤波属于线性滤波器，它可以将多元不确定信息进行融合从而得到一种最优的状态估计。卡尔曼滤波在连续变化的线性系统中表现是非常出色的，因为它考虑了系统过程中存在的一些干扰，比如模型预测干扰$Q$和测量过程干扰$R$，因此，即使系统中伴随着一些干扰，卡尔曼滤波器也可以比较准确的计算出实际的状态，并且可以对系统未来的运动状态做出合理的预测。卡尔曼滤波器的前提条件是系统是线性高斯系统，线性高斯系统需要从两个方面的理解：

1. 线性：运动方程、观测方程是线性的

2. 高斯：系统噪声服从零均值的高斯分布

综合来说也就是说高斯分布的噪声在经过状态转移之后仍然保持高斯分布。如果不满足这个线性这个前提条件，就需要使用扩展卡尔曼滤波（extended kalman filter ，EKF）（参见我的另一篇文章：扩展卡尔曼滤波（EKF）理论讲解与实例（matlab、python和C++代码））和无迹（损）卡尔曼滤波（参见我的另一篇文章：无迹（损）卡尔曼滤波（EKF）理论讲解与实例）了（Unscented Kalman Filter ，UKF）来代替了。

理论概括

如下图2.15所示，卡尔曼滤波器的信息源一方面来自传感器检测出的车辆状态信息 $\widehat{z_k}$，另一方面来自数学模型预测出的车辆状态信息 $\widehat{x_k}$，但这两方面信息源都只是间接的预测，并且伴随着一些不确定和不准确性，如果使用上所有可用的信息源，那么就能得到一个比单一的任何信息源估计更好的结果${\widehat{x_k}}^{\prime }$。卡尔曼滤波通过两个高斯分布相乘，就可以把多个不确定信息源融合在一起。传感器检测的车辆状态信息$\widehat{z_k}$在某种情况下符合一个高斯正太分布，预测模型计算出的车辆状态信息也$\widehat{x_k}$符合一个高斯正太分布，而后将这两个独立的高斯分布相乘就会得到一个新的高斯分布${\widehat{x_k}}^{\prime }$。新的高斯分布就是原来两个高斯分布重叠的区域，其均值就代表着最优的状态估计。

卡尔曼滤波的实质是预测的车辆状态$\widehat{x_k}$和测量的车辆状态$\widehat{z_k}$两个高斯分布相乘的到新的高斯分布${\widehat{x_k}}^{\prime }$， 该过程最后可以凝练成五个核心公式：

预测公式：

$$\begin{gathered} {\hat{x}}_k=F_k{\hat{x}}_{k-1}+B_k{\vec{u}}_k \\ {P}_k=F_k{P}_{k-1}{F}_k^T+Q_k \end{gathered}$$
更新公式：

$$\begin{gathered} K^{\prime }=P_k{H}_k^T(H_k{P}_k{H}_k^T+R_k{)}^{-1} \\ {\hat{x}}_k^{\prime }={\hat{x}}_k+K^{\prime }(\overrightarrow{z_k}-H_k{\hat{x}}_k) \\ {P}_k^{\prime }=P_k-K^{\prime }{H}_k{P}_k \end{gathered}$$
其中

$\widehat{x_k}$为$k$时刻的状态向量。
$F_k$为状态转移矩阵，表示将$k-1$时刻的状态向量转移至t时刻的状态向量。
$B_k$是输入控制矩阵，代表着将控制向量 $\widehat{u_k}$映射到状态向量上，统一控制向量 $\widehat{u_k}$和状态向量之间 $\widehat{x_k}$的关系。
$\widehat{u_k}$代表着控制向量，如加速度，角加速度等。
$P_k$为状态向量的协方差矩阵，代表着状态向量每个元素之间的关系。
$Q_k$表示预测状态的高斯噪声的协方差阵，它用来衡量模型的准确度，模型越准确其值越小。
$\overrightarrow{z_k}$为传感器测量值的状态向量，也就是传感器的测量结果。
$H_k$ 为转换矩阵，他将状态向量 $\widehat{x_k}$映射到测量值所在的向量空间$\overrightarrow{z_k}$。
$R_k$为测量值的高斯噪声的协方差阵，代表着传感器测量的误差。

---

公式推导

那卡尔曼滤波公式的五个公式是如何推导的呢？

1. 用均值和方差描述物体状态

怎么去描述某一时刻物体的状态呢? 状态$\widehat{x_k}$可以包含多个变量，代表任何你想表示的信息， 我们用均值和方差来表示某一时刻的状态，为啥是这两个呢? 均值就是我们定义要知道的一些变量状态，例如位置，速度，体积，温度等等。它表示随机分布的中心，即最可能的状态。方差表示一种偏离均值的程度，他可以告诉我们更多信息，利用其中一个变量值告诉我们其它变量可能的值。例如，位置和速度的关系。我们基于旧的位置来估计新位置。如果速度过高，我们可能已经移动很远了。如果缓慢移动，则距离不会很远。跟踪这种关系是非常重要的，因为它带我们更多的信息：其中一个测量值告诉了我们其它变量可能的值，这就是卡尔曼滤波的目的，尽可能地在包含不确定性的测量数据中提取更多信息！这种相关性用协方差矩阵来表示。矩阵中的每个元素 ${\Sigma }_{ij}$ 表示第 $i$个和第 $j$个状态变量之间的相关度。（协方差矩阵是一个对称矩阵，这意味着可以任意交换 $i$ 和$j$）。协方差矩阵通常用“$\Sigma$”来表示，其中的元素则表示为“ ${\Sigma }_{ij}$ ”。

所以在时刻 $k$ 物体的状态为 ：均值 $\widehat{x_k}$和协方差矩阵$P_k$ 。
$$\widehat{x_k}=\left[\begin{array}{c}position\\ velocity\end{array}\right]P_k=\left[\begin{array}{cc}{\Sigma }_{pp}& {\Sigma }_{pv}\\ {\Sigma }_{vp}& {\Sigma }_{vv}\end{array}\right]$$

2. 状态转移矩阵$F$表示系统预测

接下来，我们需要根据当前状态（$k-1$ 时刻）来预测下一状态（$k$ 时刻），怎么去预测呢?

我们可以用矩阵 $F_k$来表示这个预测过程。这个 $F_k$就是根据运动学或者动力学等物理公式得到的内在关系或者规律。

它将我们原始估计中的每个点都移动到了一个新的预测位置，如果原始估计是正确的话，这个新的预测位置就是系统下一步会移动到的位置。那我们又如何用矩阵来预测下一个时刻的位置和速度呢？下面用一个基本的运动学公式来表示：矩阵表示
$$\begin{gathered} p_k=p_{k-1}+\Delta t{v}_{k-1} \\ {v}_k=v_{k-1} \end{gathered}$$
或者表示为
$$\begin{gathered} {\hat{x}}_k=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]{\hat{x}}_{k-1} \\ =F_k{\hat{x}}_{k-1} \end{gathered}$$

现在，我们有了一个预测矩阵来表示下一时刻的状态，但是，我们仍然不知道怎么更新协方差矩阵。此时，我们需要引入另一个公式，如果我们将分布中的每个点都乘以矩阵$A$，那么它的协方差矩阵会$\Sigma$怎样变化呢？很简单，下面给出公式：
$$\begin{gathered} Cov(x)=\Sigma \\ Cov(Ax)=A\Sigma A^T \end{gathered}$$
　　结合上面两个方程得到：
$$\begin{gathered} {\hat{x}}_k=F_k{\hat{x}}_{k-1} \\ {P}_k=F_k{P}_{k-1}{F}_k^T \end{gathered}$$

3. 引入外部控制变量$u_k$

有时候，我们并没有捕捉到一切信息，可能存在外部因素会对系统进行控制，带来一些与系统自身状态没有相关性的改变。以火车的运动状态模型为例，火车司机可能会操纵油门，让火车加速。相同地，在我们机器人这个例子中，导航软件可能会发出一个指令让轮子转向或者停止。如果知道这些额外的信息，我们可以用一个向量$\overrightarrow{u_k}$来表示，将它加到我们的预测方程中做修正。
　　假设由于油门的设置或控制命令，我们知道了期望的加速度$a$根据基本的运动学方程可以得到：
$$\begin{gathered} p_k=p_{k-1}+\Delta t{v}_{k-1}+\frac{1}{2}a(\Delta t{)}^2 \\ {v}_k=v_{k-1}+a\Delta t \end{gathered}$$
以矩阵的形式表示就是：
$$\begin{gathered} {\hat{x}}_k=F_k{\hat{x}}_{k-1}+\left[\begin{array}{c}\frac{\Delta t^2}{2}\\ \Delta t\end{array}\right]a \\ =F_k{\hat{x}}_{k-1}+B_k{\vec{u}}_k \end{gathered}$$
$B_k$称为控制矩阵，$\overrightarrow{u_k}$称为控制向量（对于没有外部控制的简单系统来说，这部分可以忽略）。让我们再思考一下，如果我们的预测并不是100%准确的，该怎么办呢？

4. 引入外部干扰$Q_k$

如果这些状态量是基于系统自身的属性或者已知的外部控制作用来变化的，则不会出现什么问题。 但是，如果存在未知的干扰呢？例如，假设我们跟踪一个四旋翼飞行器，它可能会受到风的干扰，如果我们跟踪一个轮式机器人，轮子可能会打滑，或者路面上的小坡会让它减速。这样的话我们就不能继续对这些状态进行跟踪，如果没有把这些外部干扰考虑在内，我们的预测就会出现偏差。在每次预测之后，我们可以添加一些新的不确定性来建立这种与“外界”（即我们没有跟踪的干扰）之间的不确定性模型：原始估计中的每个状态变量更新到新的状态后，仍然服从高斯分布。我们可以说${\hat{x}}_{k-1}$每个状态变量移动到了一个新的服从高斯分布的区域${\hat{x}}_k$，协方差为$Q_k$。换句话说就是，我们将这些没有被跟踪的干扰当作协方差为的噪声$Q_k$来处理。这产生了具有不同协方差（但是具有相同的均值）的新的高斯分布。

我们通过简单地添加$Q_k$得到扩展的协方差，下面给出预测步骤的完整表达式：
$$\begin{gathered} {\hat{x}}_k=F_k{\hat{x}}_{k-1}+B_k{\vec{u}}_k \\ {P}_k=F_k{P}_{k-1}{F}_k^T+Q_k \end{gathered}$$
由上式可知，新的最优估计${\hat{x}}_k$是根据上一最优估计${\hat{x}}_{k-1}$预测得到的，并加上已知外部控制量${\vec{u}}_k$的修正。而新的不确定性$P_k$由上一不确定$P_{k-1}$性预测得到，并加上外部环境的干扰$Q_k$。

这里，我们对系统可能的动向有了一个模糊的估计，用均值 $\widehat{x_k}$和协方差矩阵$P_k$来表示。如果再结合传感器的数据会怎样呢？

5. 用测量值($\overrightarrow{z_k}，R_k$)来修正预测值

我们可能会有多个传感器来测量系统当前的状态，哪个传感器具体测量的是哪个状态变量并不重要，也许一个是测量位置，一个是测量速度，每个传感器间接地告诉了我们一些状态信息。注意，传感器读取的数据的单位和尺度有可能与我们要跟踪的状态的单位和尺度不一样，我们用矩阵 $H_k$ 来转换传感器的数据，使它的单位和尺度和我们的状态$x_k$一致

我们可以计算出传感器读数的分布，即均值和误差，用之前的表示方法如下式所示：
$$\begin{gathered} {\vec{u}}_{expected}=H_k{\hat{x}}_k \\ {\Sigma }_{expected}=H_k{P}_k{H}_k^T \end{gathered}$$
卡尔曼滤波的一大优点就是能处理传感器噪声，换句话说，我们的传感器或多或少都有点不可靠，并且原始估计中的每个状态可以和一定范围内的传感器读数对应起来。从测量到的传感器数据中，我们大致能猜到系统当前处于什么状态。但是由于存在不确定性，某些状态可能比我们得到的读数更接近真实状态。我们将这种不确定性（例如：传感器噪声）用协方差$R_K$表示，该分布的均值就是我们读取到的传感器数据，称之为$\overrightarrow{z_k}$。
现在我们有了两个高斯分布，一个是在预测值附近，一个是在传感器读数附近。

我们必须在预测值（粉红色）和传感器测量值（绿色）之间找到最优解。那么，我们最有可能的状态是什么呢？如果我们想知道这两种情况都可能发生的概率，将这两个高斯分布相乘就可以了。

6. 融合高斯分布公式

先以一维高斯分布来分析，具有方差 ${\sigma }^2$ 和 $u$ 的高斯曲线可以用下式表示：

如果把两个服从高斯分布的函数相乘会得到什么呢？

将式（9）代入到式（10）中（注意重新归一化，使总概率为1）可以得到：

将式（11）中的两个式子相同的部分用 $K$ 表示：

下面进一步将式（12）和（13）写成矩阵的形式，如果 $\Sigma$ 表示高斯分布的协方差，$\vec{u}$ 表示每个维度的均值，则：

矩阵 $K$ 称为卡尔曼增益，下面将会用到。

7. 将所有公式整合起来

我们有两个高斯分布，预测部分$(u_0,{\Sigma }_0)=(H_k\widehat{x_k},H_k{P}_k{H}_k^T)$ ，和测量部分$(u_1,{\Sigma }_1)=(\overrightarrow{z_k},R_k)$，将它们放到式（15）中算出它们之间的重叠部分：

由式（14）可得卡尔曼增益为：

将式（16）和式（17）的两边同时左乘矩阵的逆（注意 $K$里面包含了$H_k$ ）将其约掉，再将式（16）的第二个等式两边同时右乘矩阵 $H_k^T$ 的逆得到以下等式：

上式给出了完整的更新步骤方程。 ${\widehat{x_k}}^{\prime }$ 就是新的最优估计，我们可以将它和放 $P_k^{{}^{\prime }}$到下一个预测和更新方程中不断迭代。

---

卡哇伊~~~

---

调整参数

卡尔曼滤波的核心代码虽然就短短的几行，但参数的确定却不是那么容易 。在很多工业控制领域，比如飞船的控制、卫星的控制等都有专门的负责调参的工程师甚至是一个团队，可见卡尔曼滤波参数整定的重要程度。从某种意义上说，调参更像是一门艺术。针对于同一套工程模型，这些参数理论上应该可以统一，但是不同环境下系统的过程噪声以及测量噪声有可能不同，因此还是需要实际测试，以便于找到参数的最优值。可以参考这篇文章参数调整

其中大部分参数需要根据实际应用情况做调整，例如$Q_k$值、$R_k$值和$P_k$的值，下面逐一进行介绍。$Q_k$值为过程噪声，用来衡量预测模型计算的误差大小，如果其值越小系统对预测模型的信任度也就越高，系统也越发容易收敛，说明模型设计的较为合理。 具体的，如果$Q_k$设置为零，说明系统非常相信预测模型不相信测量值，随着$Q_k$值越来越大，系统对预测模型的信任度就越来越低，对测量值的信任度就越来越高，更甚者，如果$Q_k$值无穷大，那么系统只信任测量值不相信预测值。$R_k$值为测量噪声，代表着传感器测量误差，其值用来衡量测量误差的大小。 如果$R_k$太大，整个系统的响应时间就会变的比较慢，原因是系统对新的传感器测量的值不是那么相信，从而导致新来的值对整个系统影响不大。如果越小系统收敛越快，但如果$R_k$越小，系统反应就越快，收敛速度也会越快，更甚者，如果$R_k$过小就有可能导致系统出现震荡。$R_k$的确定一般有如下两种方法，其一通过对传感器进行大量测试，记录出每个数据的输出，那么这些大量输出数据近似符合正态分布，根据$3\sigma$原则，取该正态分布的$(3\sigma {)}^2$作为$R$的初始化值。其二可以查看说明书，传感器出厂时，厂家一般都会给出传感器测量误差值。最后一个关键参数是$P_k$，它是状态向量${\hat{x}}_k$之间元素误差的协方差初始值，表示卡尔曼滤波系统对当前预测状态向量的置信度，其值越小说明系统就越相信当前预测状态。$P_k$的大小决定了初始收敛速度。$P_k$值的大小只是影响系统的初始收敛速度，在调试的时候，一般设一个较小的$P_k$值，这样可以得到一个比较快的收敛速度，随着卡尔曼滤波的迭代，$P_k$的值会随着系统的迭代不断的改变，当系统进入到一个稳态的状态之后，$P_k$的值也会收敛成一个最小的协方差矩阵。对于$Q_k$值和$R_k$值，可以先将$Q_k$值从小往大调整，将$R_k$从大往小调整，这样系统可以较快的得到收敛，然后根据系统的收敛速度，固定一个值再去调整另外一个值。

应用CA模型

​ matlab和python例程1的转换矩阵只考虑了位置这个量测值（python例程2考虑了速度的量测值）。 参数设置$p\_var$的值为0.1 ，$q\_var$的值为0.01 ， ${\sigma }_{\gamma }$的值为0.1

代码例程（matlab）

clear;
clc;
close all;

%% 构建数据集
dt=0.1;
t=[0:dt:10];
N=length(t);
v_0=10;
a_0=2;
theta=pi/3;
px=v_0*cos(theta)*t+0.5*a_0*t.^2;
py=v_0*sin(theta)*t+0.5*a_0*t.^2;
%% 添加噪声
noise=1.5*randn(1,length(px));%均值为0 方差为2.25的正态分布
noise_px=px+noise; 
noise=1.5*randn(1,length(py));%均值为0 方差为2.25的正态分布
noise_py=py+noise; 

dataset=[px;py];
noise_dateset=[noise_px;noise_py];

% plot(dataset(1,:),dataset(2,:));
% hold on ;
% plot(noise_dateset(1,:),noise_dateset(2,:));
% legend('origin','noise')% ,' estimate'

%% 设置卡尔曼滤波矩阵的参数
%CA模型
X=[noise_dateset(1,1);noise_dateset(2,1);0;0;0;0]; %初始状态 X=[位置px;位置py]
p_var=0.1;
P=diag(ones(1,6))*p_var; %状态协方差矩阵
rank=length(P);%矩阵的秩
F=[   1 0 dt 0 dt^2/2 0;
      0 1 0 dt 0 dt^2/2;
      0 0 1 0 dt 0;
      0 0 0 1 0 dt;
      0 0 0 0 1 0;
      0 0 0 0 0 1 ]; %状态转移矩阵 
  
H=[1 0 0 0 0 0;
   0 1 0 0 0 0];%传感器提供的观测矩阵

r_var=0.1;
R=diag(ones(1,2))*r_var;%传感器的观测噪声协方差矩阵

q_var=0.01;
Q=diag(ones(1,6))*q_var; %状态转移协方差矩阵

%% 卡尔曼滤波
for i = 2:N 
  X_ = F*X(:,i-1); %基于上一状态预测当前状态  X_为t时刻状态预测(这里没有控制)
  P_ = F*P(:,rank*(i-1)-5:rank*(i-1))*F'+Q;%更新协方差  Q系统过程的协方差
  %% 计算卡尔曼增益
  K = P_*H'/(H*P_*H'+R);
  %% 更新
  X(:,i) = X_+K*(noise_dateset(:,i)-H*X_);% 得到当前状态的最优化估算值  增益乘以残差
  P(:,rank*i-5:rank*i) = (eye(rank)-K*H)*P_;%更新K状态的协方差 
end

%% 绘图
figure;
plot(dataset(1,:),dataset(2,:),'r-','LineWidth',1)
hold on;
plot(X(1,:),X(2,:),'g-','LineWidth',1.5); 
hold on;
plot(noise_dateset(1,:),noise_dateset(2,:),'b-','LineWidth',1)
xlabel('时间/s'),ylabel('运动距离 m')
legend('measure','kalman','noise measure')
title('CV模型卡尔曼滤波')
grid on %显示网格 

代码例程1（python）

#!/usr/bin/python
# -*- coding: utf-8 -*-
'''
this file can single run main function
'''
from numpy.linalg import inv
from numpy import identity
from numpy import matrix, diag, random
import numpy as np
import matplotlib.pyplot as plt
import random

class CVKalmanFilter:
    """
    Simple Kalman filter
    Control term has been omitted for now
    """
    def __init__(self, X, P, F, Q, Z, H, R):
        """
        Initialise the filter
        Args:
            X: State estimate
            P: Estimate covaConfigureriance
            F: State transition model
            Q: Process noise covariance
            Z: Measurement of the state X
            H: Observation model
            R: Observation noise covariance
        """
        self.X = X
        self.P = P
        self.F = F
        self.Q = Q
        self.Z = Z
        self.H = H
        self.R = R


    def predict(self, X, P, w=0):
        """
        Predict the future state
        Args:
            X: State estimate
            P: Estimate covariance
            w: Process noise
        Returns:
            updated (X, P)
        """
        # Project the state ahead
        X = np.dot(self.F ,X) + w
        P = np.dot(np.dot(self.F , P) , self.F.T) + self.Q

        return(X, P)

    def update(self, X, P, Z):
        """
        Update the Kalman Filter from a measurement
        Args:
            X: State estimate
            P: Estimate covariance
            Z: State measurement
        Returns:
            updated (X, P)
        """
        S = np.dot(np.dot(self.H, P), self.H.T) + self.R
        K = np.dot(np.dot(P, self.H.T), np.linalg.inv(S))
        
        y = Z - np.dot(self.H, X)
        X = X + np.dot(K, y)
        P = np.dot((identity(P.shape[1]) - np.dot(K, self.H)), P)
        return (X, P)


# Testing
if __name__ == "__main__":

    # data set
    # own set,The format is  dataset=[time px py];
    dataset=[]
    # Time step size
    dt = 0.1
    # constuct dataset
    t=np.arange(0, 10, dt)
    v_0=10
    a_0=2
    theta=np.pi/3
    px=v_0*np.cos(theta)*t+0.5*a_0*t*t
    py=v_0*np.sin(theta)*t+0.5*a_0*t*t
  

    # add gauss noise
    mu = 0
    sigma = 1.5
    noise_px=np.zeros(np.array(px).shape)
    noise_py=np.zeros(np.array(py).shape)
    for i in range(len(px)):
        noise_px[i] = px[i] + random.gauss(mu,sigma)
        noise_py[i] = py[i] + random.gauss(mu,sigma)
    
    # plt.plot(px,py,noise_px,noise_py)
    # plt.show()

    for i in range(len(t)):
        c=[]
        c.append(t[i])
        c.append(noise_px[i])
        c.append(noise_py[i])
        dataset.append(c)
   
    
    
    # Standard deviation of random accelerations
    sigma_a = 0.2
    # Standard deviation of observations
    sigma_z = 0.2

    # State vector: [[Position_x],[Position_x] [velocity_x]  [velocity_y]
    # [acc_x]  [acc_y]]
    X = np.zeros(6).reshape(6,1)
 
    # State transition model 6*6
    F = np.diag([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
   
    # Initial state covariance 6*6
    p_var = 0.1
    P = np.diag([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
    P = P * p_var
   
    # Observation vector
    Z = np.zeros(2).reshape(2,1)
  
    # Observation model
    H = np.array([[ 1.,  0.,  0.,  0.,  0.,  0.],
            [ 0.,  1.,  0.,  0.,  0.,  0.]])
   
    # Observation covariance
    r_var=0.25
    R = np.diag([1.0, 1.0])
    R = R * r_var
   
    # Process noise covariance matrix
    q_var = 0.01
    Q = np.diag([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
    Q = Q * q_var
 
    # Initialise the filter
    kf = CVKalmanFilter(X, P, F, Q, Z, H, R)


    # initial the first value
    
    pre_time=dataset[0][0]
    X[0,0]=dataset[0][1]
    X[1,0]=dataset[0][2]
    
    # initial result value
    kf_result = []
    kf_result.append(X)

    # Number of measurement_step
    measurement_step = len(dataset)

    for i in range(1, measurement_step):
        
        t_measurement = dataset[i]
        delta_ts = (t_measurement[0] - pre_time)
        pre_time = t_measurement[0]
        # print(delta_ts,kf.F)
        kf.F[0, 2] = delta_ts
        kf.F[0, 4] = 0.5 * delta_ts * delta_ts
        kf.F[1, 3] = delta_ts
        kf.F[1, 5] = 0.5 * delta_ts * delta_ts
        kf.F[2, 4] = delta_ts
        kf.F[3, 5] = delta_ts

        # Predict
        (X, P) = kf.predict(X, P)
        # Update
        m_x = t_measurement[1]
        m_y = t_measurement[2]
        Z = np.array([[m_x], [m_y]])
        (X, P) = kf.update(X, P, Z)
        # save the filter position
        kf_result.append(X)
    
    # plot
    print(len(kf_result),len(t))
    kf_global_x_list = []
    kf_global_y_list = []
    kf_velocity_x_list = []
    kf_velocity_y_list = []
    kf_acc_x_list = []
    kf_acc_y_list = []
    
    for i in range(len(kf_result)):
        # print kf_result[i], kf_result[i][0],kf_result[i][1]   
        kf_global_x_list.append(kf_result[i][0])
        kf_global_y_list.append(kf_result[i][1])
        kf_velocity_x_list.append(kf_result[i][2])
        kf_velocity_y_list.append(kf_result[i][3])
        kf_acc_x_list.append(kf_result[i][4])
        kf_acc_y_list.append(kf_result[i][5])
        
    # 测试
    plt.figure
    plt.plot(px,py, 'r-',  label='measure') 
    plt.plot(noise_px,noise_py, 'b-',  label='noise') 
    plt.plot(kf_global_x_list,kf_global_y_list, 'g-',  label='kf estimate') 
    plt.title("Position") 
    plt.xlabel("time") 
    plt.ylabel("d(m)") 
    plt.legend()
    plt.grid(color="k", linestyle=":")
    plt.show()

代码例程2（python）

当转换矩阵H为
$$\begin{gathered} H=[[1,0,0,0,0,0], \\ [0,1,0,0,0,0], \\ [0,0,1,0,0,0], \\ [0,0,0,1,0,0]] \end{gathered}$$

声明：该代码需要自己完善才能运行。

from numpy.linalg import inv
from numpy import identity
from numpy import matrix, diag, random
import numpy as np

class CVKalmanFilter:
    """
    Simple Kalman filter
    Control term has been omitted for now
    """
    def __init__(self, X, P, F, Q, Z, H, R):
        """
        Initialise the filter
        Args:
            X: State estimate
            P: Estimate covaConfigureriance
            F: State transition model
            Q: Process noise covariance
            Z: Measurement of the state X
            H: Observation model
            R: Observation noise covariance
        """
        self.X = X
        self.P = P
        self.F = F
        self.Q = Q
        self.Z = Z
        self.H = H
        self.R = R


    def predict(self, X, P, w=0):
        """
        Predict the future state
        Args:
            X: State estimate
            P: Estimate covariance
            w: Process noise
        Returns:
            updated (X, P)
        """
        # Project the state ahead
        X = np.dot(self.F ,X) + w
        P = np.dot(np.dot(self.F , P) , self.F.T) + self.Q

        return(X, P)

    def update(self, X, P, Z):
        """
        Update the Kalman Filter from a measurement
        Args:
            X: State estimate
            P: Estimate covariance
            Z: State measurement
        Returns:
            updated (X, P)
        """
        S = np.dot(np.dot(self.H, P), self.H.T) + self.R
        K = np.dot(np.dot(P, self.H.T), np.linalg.inv(S))
        
        y = Z - np.dot(self.H, X)
        X = X + np.dot(K, y)
        P = np.dot((identity(P.shape[1]) - np.dot(K, self.H)), P)
        return (X, P)


# Testing
if __name__ == "__main__":

    # data set 
    # own set,The format is  dataset=[time px py vx vy ax ay];
    dataset=[]
    # Time step size
    # dt = 0.1
    # Standard deviation of random accelerations
    sigma_a = 0.2
    # Standard deviation of observations
    sigma_z = 0.2

    # State vector: [[Position_x],[Position_x] [velocity_x]  [velocity_y]
    # [acc_x]  [acc_y]]
    X = np.zeros(6).reshape(6,1)
  
    # State transition model 6*6
    F = np.diag([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
    
    # Initial state covariance 6*6
    p_var = 0.1
    P = np.diag([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
    P = P * p_var
    
    # Observation vector
    Z = np.zeros(4).reshape(4,1)
    
    # Observation model
    H = np.array([[ 1.,  0.,  0.,  0.,  0.,  0.],
                [ 0.,  1.,  0.,  0.,  0.,  0.],
                [ 0.,  0.,  1.,  0.,  0.,  0.],
                [ 0.,  0.,  0.,  1.,  0.,  0.]])
    
    # Observation covariance
    r_var=0.25
    R = np.diag([1.0, 1.0])
    R = R * r_var
    
    # Process noise covariance matrix
    q_var = 0.01
    Q = np.diag([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
    Q = Q * q_var
    
    # Initialise the filter
    kf = CVKalmanFilter(X, P, F, Q, Z, H, R)
	
    # initial the first value
    pre_time=dataset[0][0]
    X[0,0]=dataset[0][1]
    X[1,0]=dataset[0][2]
    X[2,0]=dataset[0][3]
    X[3,0]=dataset[0][4]
    X[4,0]=dataset[0][5]
    X[5,0]=dataset[0][6]
    
    # initial result value
    kf_result = []
    kf_result.append(X)
    # Number of measurement_step
    measurement_step = len(dataset)
    
    for i in range(1, measurement_step):
       
        t_measurement = dataset[i]
        
        delta_ts = (t_measurement[0] - pre_time) 
        pre_time = t_measurement[0]
        F[0, 2] = delta_ts;
        F[0, 4] = 0.5 * delta_ts * delta_ts;
        F[1, 3] = delta_ts;
        F[1, 5] = 0.5 * delta_ts * delta_ts;
        F[2, 4] = delta_ts;
        F[3, 5] = delta_ts;

        # Predict
        (X, P) = kf.predict(X, P)
        # Update
        m_x = t_measurement[1]
        m_y = t_measurement[2]
        m_Vx = t_measurement[3]
        m_Vy = t_measurement[4]
        Z = np.array([[m_x], [m_y],[m_Vx],[m_Vy]])
        (X, P) = kf.update(X, P, Z)
        kf_result.append(X)
        # Update the actual position

应用CV模型

$$\begin{gathered} \widehat{x_k}=\left[\begin{array}{c}position\\ velocity\end{array}\right]P_k=\left[\begin{array}{cc}{\Sigma }_{pp}& {\Sigma }_{pv}\\ {\Sigma }_{vp}& {\Sigma }_{vv}\end{array}\right] \\ {p}_k=p_{k-1}+\Delta t{v}_{k-1} \\ {v}_k=v_{k-1} \\ {\hat{x}}_k=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]{\hat{x}}_{k-1} \\ =F_k{\hat{x}}_{k-1} \end{gathered}$$

matlab代码

Z=(1:2:200); %观测值 
noise=5*randn(1,100); %均值为0 方差为25的正态分布
moise_Z=Z+noise;%模拟的实际观测值

%CV模型
X=[moise_Z(1);0]; %初始状态 X=[位置;速度]
P=[1 0;0 1]; %状态协方差矩阵
rank=length(P);%矩阵的秩
T=1;
F=[1 T;0 1]; %状态转移矩阵 
Q=[0.0001,0;0 , 0.0001]; %状态转移协方差矩阵
H=[1,0];%传感器提供的观测矩阵
R=1;%传感器的观测噪声协方差矩阵

%% 卡尔曼滤波
for i = 2:100 
  X_ = F*X(:,i-1); %基于上一状态预测当前状态  X_为t时刻状态预测(这里没有控制)
  P_ = F*P(:,rank*(i-1)-1:rank*(i-1))*F'+Q;%更新协方差  Q系统过程的协方差
  %% 计算卡尔曼增益
  K = P_*H'/(H*P_*H'+R);
  %% 更新
  X(:,i) = X_+K*(moise_Z(i)-H*X_);% 得到当前状态的最优化估算值  增益乘以残差
  P(:,rank*i-1:rank*i) = (eye(rank)-K*H)*P_;%更新K状态的协方差 
end

%% 绘图
figure;
plot(Z,'r-','LineWidth',1)
hold on;
plot(X(1,:),'g-','LineWidth',1.5); 
hold on;
plot(moise_Z,'b-','LineWidth',1)
xlabel('时间/s'),ylabel('运动距离 m')
legend('measure','kalman','noise measure')
title('CV模型卡尔曼滤波')
grid on %显示网格 

python代码

声明：该代码需要自己完善才能运行。

import numpy as np
import pandas as pd

def kalman_filter(zk, xk, A=np.matrix(1), B=np.matrix(0), Pk=1, 
                    uk=np.array(0), wk=0, Q=0.1, R=1, H=np.matrix(1)):
    """Performs Kalman Filtering on pandas timeseries data.
    :param: zk (pandas timeseries): input data
    :param: xk (np.array): a priori state estimate vector
    :param: A (np.matrix): state transition coefficient matrix
    :param: B (np.matrix): control coefficient matrix
    :param: Pk (np.matrix): prediction covariance matrix
    :param: uk (np.array): control signal vector
    :param: wk (float): process noise (has covariance Q)
    :param: Q (float): process covariance
    :param: R (float): measurement covariance
    :param: H (np.matrix):  transformation matrix
    :return: output (np.array): kalman filtered data
    """
    output = np.zeros(len(zk))
    for k, t in enumerate(zk.index):
        # time update (prediction)
        xk = A*xk + B*uk + wk # Predict state
        zk_pred = H*xk # Predict measurement
        Pk = A*Pk*A.T + Q # Predict error covariance
        # measurement update (correction)
        vk = zk[t] - zk_pred # Measurement residual
        S = (H*Pk*H.T) + R # Prediction covariance
        Kk = (Pk*H.T) / S # Compute Kalman Gain
        xk = xk + (Kk * vk) # Update estimate with gain * residual
        Pk = (1 - Kk*H)*Pk # Update error covariance
        output[k] = xk.item()
    return output

参考链接

1. 常用的运动模型