无人驾驶1：卡尔曼滤波原理及实现(以无人车观测为实例)

朴灿烈づ我的快乐病毒、 2023-06-27 06:10 15阅读 0赞

本系列文章，参考如下资料：

我真正理解卡尔曼滤波是看这篇文章，建议直接看原文：  
1[http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/][http_www.bzarg.com_p_how-a-kalman-filter-works-in-pictures]  
对上文的翻译：

2.[https://zhuanlan.zhihu.com/p/39912633][https_zhuanlan.zhihu.com_p_39912633]

3.优达学城无人驾驶纳米课程

4.《概率机器人》Sebastian Trun, Wofram Burgard, Dieter Fox，机械工业出版社

5.《无人驾驶原理与实践》申泽邦 雍宾宾 周庆国 李良 李冠憬 机械工业出版社

# 1.数学背景知识 #

## 1.1 什么是高斯分布 ##

高斯分布(Gaussian distribution):  
也叫正态分布(Normal distribution)，是一个在自然和社会科学中，非常广泛存在的一个连续概率分布。

概率密度函数为：  
 f ( x ) = 1 σ ( 2 π ) e − ( x − μ ) 2 2 σ 2 f(x) = \\frac\{1\} \{\\sigma\\sqrt\{(2\\pi)\}\} e^\{-\\frac\{(x-\\mu)^2\}\{2\\sigma^2\}\} f(x)=σ(2π)1e−2σ2(x−μ)2

如果把这个空间称为x， 那么高斯函数的性质可以用两个参数来反映，一个时均值 μ \\mu μ,一个是宽度 σ 2 \\sigma^2 σ2

![1D高斯分布图][1D]

## 1.2 高斯分布性质 ##

σ 2 \\sigma^2 σ2 反映曲线宽度， σ 2 \\sigma^2 σ2 越大，曲线越宽，概率分布越分散，确定性越低，反之确定性越高。如下图所示  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70]

## 1.3 高斯分布计算 ##

假设 μ \\mu μ=10,  σ 2 \\sigma^2 σ2=4, 求x的概率？

from math import *
    def gaussian(mu,sigma2,x):
        return 1.0/sqrt(2.*pi*sigma2)*exp(-.5*(x-mu)**2/sigma2)

print(gaussian(10.,4.,8.))
    print(gaussian(10.,4.,10.))
    print(gaussian(10.,4.,12.))

![在这里插入图片描述][20200204173846977.png]  
注：当x= μ \\mu μ时，取得最大值；

在 Python 中计算曲线下方面积:

from scipy.stats import norm
    
    def gaussian_probability(mean, stdev, x_low, x_high):
        return norm(loc = mean, scale = stdev).cdf(x_high) - norm(loc = mean, scale = stdev).cdf(x_low)

## 1.4 高斯分布乘法 ##

假设有如下场景，黑色曲线为一个车辆在一个维度上的初始概率分布(先验概率)，蓝色曲线为当前场景一次测量的概率分布(条件概率)  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 1]  
那么要计算当前车辆最佳估计，根据贝叶斯定理，后验概率=先验概率\*条件概率  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 2]  
具体计算方法如下：  
![测量更新][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 3]  
p(x): 先验概率  
p(z|x): 测量更新  
p(x|z): 后验概率  
注：方差项不受均值影响，只使用之前的方差，得到一个更陡峭的方差；  
代码实现：

# Write a program to update your mean and variance
    # when given the mean and variance of your belief
    # and the mean and variance of your measurement.
    # This program will update the parameters of your
    # belief function.
    
    def update(mean1, var1, mean2, var2):
        new_mean = (mean1*var2+mean2*var1)/(var1+var2)
        new_var =1./(1./var1+1./var2)
        return [new_mean, new_var]

例如：  μ 1 \\mu\_1 μ1=10,  σ 1 2 \\sigma\_1^2 σ12=4;  μ 2 \\mu\_2 μ2=12,  σ 2 2 \\sigma\_2^2 σ22=4;  
![在这里插入图片描述][2020020418134524.png]  
![gaussie乘法][gaussie]  
一个有点违反直觉的例子，假设有两个方差相同，均值不同的分布如下图，相乘结果如何：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 4]  
结果是下图红色曲线表征的分布：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 5]

## 1.5 高斯分布加法 ##

假设如下场景，一个车辆如下图蓝色曲线表征初始分布，然后移动u(移动量本身有误差，服从高斯分布)，预计移动后分布时怎样？  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 6]  
计算方法：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 7]  
代码实现：

def predict(mean1, var1, mean2, var2):
        new_mean = mean1 + mean2
        new_var = var1 + var2
        return [new_mean, new_var]

## 1.6多维高斯分布 ##

实际应用中，需要观测的变量往往不只一个，设计多个维度，写成矩阵形式如下：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 8]  
观测变量有D个组成一个D维向量, 不确定性用一个协方差矩阵表示

# 2. 卡尔曼滤波 #

## 2.1 什么是卡尔曼滤波 ##

--------------------

在物体跟踪、预测类的应用中，通常需要对目标状态进行状态估计预测。

因为在实际场景中通常需要持续观测、预测目标的运动和发展情况，以对当前状态采取更合适的决策。  
而实际的传感器测量，由于测量的误差和噪声存在，测量值不完全可信。这时候，就可以采用**概率学**和**统计学**的方法来分析统计和估计状态量。

**卡尔曼滤波**就是这样一种结合**预测**（先验分布）和**测量更新**(似然估计)的状态估计算法。

![kalman filter][]  
卡尔曼滤波总是包含两个大的部分，即测量更新和预测更新，下面通过实际案例来解释其原理；

## 2.2 一维卡尔曼滤波应用场景 ##

假设有一辆汽车沿直线行驶，测量位置值有些许误差和干扰，应用卡尔曼滤波来提高定位的准确度。

**step0 初始值**: 假设汽车初始位置为（ μ 0 \\mu\_0 μ0， σ 0 2 \\sigma\_0^2 σ02）, 其概率分布为  
 g a u s s i a n 0 = g a u s s i a n ( μ 0 , σ 0 2 , x ) gaussian\_0=gaussian(\\mu\_0,\\sigma\_0^2,x) gaussian0=gaussian(μ0,σ02,x)  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 9]

**step1 测量更新**: 测量更新传感器测得汽车位置数据为（ μ 1 \\mu\_1 μ1， σ 1 2 \\sigma\_1^2 σ12），即测量概率分布为  
 g a u s s i a n 1 = g a u s s i a n ( μ 1 , σ 1 2 , x ) gaussian\_1=gaussian(\\mu\_1,\\sigma\_1^2,x) gaussian1=gaussian(μ1,σ12,x)  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 10]

根据贝叶斯定律，测量后估计值修正为  
 g \_ u p d a t e = g a u s s i a n 0 ∗ g a u s s i a n 1 g\\\_update = gaussian\_0\*gaussian\_1 g\_update=gaussian0∗gaussian1  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 11]

按1.4高斯乘法实现如下

def update(mean1, var1, mean2, var2):
        new_mean = (mean1*var2+mean2*var1)/(var1+var2)
        new_var =1./(1./var1+1./var2)
        return [new_mean, new_var]

**step2 预测更新**：根据1.5高斯分布加法原理，实现如下

def predict(mean1, var1, mean2, var2):
        new_mean = mean1 + mean2
        new_var = var1 + var2
        return [new_mean, new_var]

在实际持续运动过程中，会对step1/step2循环执行；一个具体案例实现如下：

measurements = [5., 6., 7., 9., 10.] #测量值
    motion = [1., 1., 2., 1., 1.] #运动变化量
    measurement_sig = 4.  #测量高斯噪声
    motion_sig = 2.       #运动过程噪声
    mu = 0.              
    sig = 10000.           #车辆初始位置，sig=10000表示不确定性很高
    
    # Write a program that will iteratively update and
    # predict based on the location measurements 
    # and inferred motions shown below. 
    def update(mean1, var1, mean2, var2):
        new_mean = float(var2 * mean1 + var1 * mean2) / (var1 + var2)
        new_var = 1./(1./var1 + 1./var2)
        return [new_mean, new_var]
    
    def predict(mean1, var1, mean2, var2):
        new_mean = mean1 + mean2
        new_var = var1 + var2
        return [new_mean, new_var]
    # Insert code here
    for i in range(len(measurements)):
        mu,sig = update(mu,sig,measurements[i],measurement_sig) #测量更新，降低不确定性
        print("update:", [mu, sig])
        mu,sig = predict(mu,sig,motion[i],motion_sig)   #预测更新，增大不确定性
        print("predict:", [mu, sig])
    
    print(mu, sig)

执行结果：  
![卡尔曼][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 12]  
由上可知：每次执行update函数，不确定性降低；而执行完predict不确定性增大；

ps:  
在任何含有**不确定性**信息的**动态系统**中，使用卡尔曼滤波，可以对下一步的趋势走向作出**有根据的预测**，即使总是伴随各种测量干扰和误差，卡尔曼滤波能指示出最接近真实发生的情况。

在**连续变化**的系统中，使用卡尔曼滤波效果非常理想，且它占用内存小(只保存前一个测量时刻的状态量，不需要保留其他历史数据)，计算速度快，适合运用于解决实时问题和嵌入式系统。

## 2.3 多维卡尔曼滤波 ##

### 1 多元高斯分布数学知识 ###

协方差定义了分布的集中程度，如图，二维高斯分布图，分别表示三种不同相关性，沦落线某个分量上的范围，表示该向量的不确定性大小；

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 13]

如果轮廓线是斜的，说明表示两个维度的不确定性是相关的；

### 2.卡尔曼滤波器可以估算隐藏变量 ###

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 14]  
进行估算高维度空间时，当观测向量中有某些分量不容易测量时，可以根据其他测量值估算出来，这是卡尔曼滤波器在人工智能和控制理论方面，成为如此流行算法的原因之一。

下面实例解析其原理![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 15]  
如图示，假设需要观测两个分量分别为位置和速度，但是传感器只能测量位置一个分量。  
a.初始值，时刻t=1时观测到无人车位于1处，速度未知，所以此时二维高斯分布如图蓝色轮廓线所示；  
b.当速度为0时，下一时刻位置依然为1；  
当速度为1时，下一时刻位置为2,；  
当速度为2时，下一时刻位置为3；

以此类推描绘出下一个时刻所有可能的分布，如上图红色轮廓线部分；

该分布单独投影到位置分量，得不到任何有效信息，单独投影到速度分量亦得不出有效信息，但是其反映了位置和速度的关联关系;

即当测量其中某一个值时（比如测量到位置为2时），由于速度未知，其分布如上图黄色部分描绘；

根据贝叶斯定理，可以得到该时刻的位置，速度二维高斯分布如上图黑色部分轮廓描绘；即可以估算出速度值；

### 3.完整卡尔曼滤波实现 ###

卡尔曼滤波是一个递推算法，主要包括五个步骤：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 16]

#### step1:状态预测 ####

x k ′ = a x k − 1 ′ + w k x\_k^\{'\}=ax\_\{k-1\}^\{'\} + w\_k xk′=axk−1′\+wk

a: 过程模型(Process Model), 常见为物理模型；  
 w k w\_k wk:过程噪声(Process Noise)，为简化计算，暂时忽略;  
 x k − 1 ′ x\_\{k-1\}^\{'\} xk−1′:k-1时刻观测向量值;

#### step2:计算预测误差 ####

p k ′ = a p k a T p\_k^\{'\} = ap\_ka^T pk′=apkaT

#### step3:计算卡尔曼增益 ####

K k = p k ′ / ( p k ′ + r ) K\_k = p\_k^\{'\}/(p\_k^\{'\}+r) Kk=pk′/(pk′\+r)

K k K\_k Kk: 表示卡尔曼增益  
 p k ′ p\_k^\{'\} pk′:k时刻预测误差  
r: 测量噪声,通常满足高斯分布(r, σ \\sigma σ), r可以通过测量，或者直接从传感器厂商获得；

#### step4:计算最优估计值 ####

X k ^ = x k ′ + K k ( z k − x k ′ ) \\hat\{X\_k\} = x\_k^\{'\} + K\_k(z\_k-x\_k^\{'\}) Xk^=xk′\+Kk(zk−xk′)

X k ^ \\hat\{X\_k\} Xk^ : k时刻最优估计；  
 x k ′ : 过 程 模 型 计 算 的 k 时 刻 预 测 值 ； x\_k^\{'\}: 过程模型计算的k时刻预测值； xk′:过程模型计算的k时刻预测值；  
 z k z\_k zk: k时刻测量值；

上式可改写成

X k ^ = ( 1 − K k ) x k ′ + K k z k \\hat\{X\_k\} =(1-K\_k) x\_k^\{'\} + K\_kz\_k Xk^=(1−Kk)xk′\+Kkzk

从这个公式可以看出，卡尔曼增益实际上就是一个权重，考虑极端情况  
当 K k = 0 K\_k=0 Kk=0,  X k ^ = x k ′ \\hat\{X\_k\} =x\_k^\{'\} Xk^=xk′: 表示当前测量极不可信，直接用预测值当作最优估算值；  
当 K k = 1 K\_k=1 Kk=1,  X k ^ = z k ′ \\hat\{X\_k\} =z\_k^\{'\} Xk^=zk′: 表示当前测量极其可信，直接用测量值当作最优估算值；  
当 K k K\_k Kk介于0～1之间，表示预测值和测量值都有可信度, 根据 K k K\_k Kk所示权重取值；

#### step5:计算最优估计误差 ####

p k = ( 1 − K k ) p k ′ p\_k = (1-K\_k)p\_k^\{'\} pk=(1−Kk)pk′

p k p\_k pk:k时刻最优估算值误差

注：该处 p k p\_k pk用于下一个递推循环的预测更新；

用向量形式表示，对应公式如下图  
![卡尔曼滤波公式][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 17]  
注：公式不需要记忆，用时查询即可，重点是确定相关参数；

#### 4.一个实例代码实现： ####

############################################
    ### use the code below to test your filter!
    ############################################
    
    measurements = [1, 2, 3] #只能测量位置信号
    
    #初始位置，速度值均为值，不确定性很高
    x = matrix([[0.], [0.]]) # initial state (location and velocity)
    P = matrix([[1000., 0.], [0., 1000.]]) # initial uncertainty
    
    u = matrix([[0.], [0.]]) # external motion #
    F = matrix([[1., 1.], [0, 1.]]) # next state function 状态转换函数
    H = matrix([[1., 0.]]) # measurement function 只能测量位置
    R = matrix([[1.]]) # measurement uncertainty 传感器噪声
    I = matrix([[1., 0.], [0., 1.]]) # identity matrix

以上为相关参数及测量值，运用卡尔曼滤波求位置和速度的最佳估计

########################################
    
    # Implement the filter function below
    
    def kalman_filter(x, P):
        for n in range(len(measurements)):
            # measurement update
            Z = matrix([[measurements[n]]])
            print("z======================================")
            print(Z)
            y = Z - (H*x)
            S = H*P*H.transpose() + R
            K = P*H.transpose()*S.inverse()
            x = x+(K*y)
            P = (I-K*H)*P
    
            
            # prediction
            x = F*x + u
            P = F*P*F.transpose()
            print("x= ")
            x.show()
            print("p= ")
            P.show()
            print()
            
        return x,P

执行结果如下

print(kalman_filter(x, P))

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 18]  
由结果知，估算出位置，速度分别为3.99, 0.99，实现过程用到的矩阵类如下：

# Write a function 'kalman_filter' that implements a multi-
    # dimensional Kalman Filter for the example given
    
    from math import *
    class matrix:
        # implements basic operations of a matrix class
        
        def __init__(self, value):
            self.value = value
            self.dimx = len(value)
            self.dimy = len(value[0])
            if value == [[]]:
                self.dimx = 0
        
        def zero(self, dimx, dimy):
            # check if valid dimensions
            if dimx < 1 or dimy < 1:
                raise ValueError("Invalid size of matrix")
            else:
                self.dimx = dimx
                self.dimy = dimy
                self.value = [[0 for row in range(dimy)] for col in range(dimx)]
        
        def identity(self, dim):
            # check if valid dimension
            if dim < 1:
                raise ValueError("Invalid size of matrix")
            else:
                self.dimx = dim
                self.dimy = dim
                self.value = [[0 for row in range(dim)] for col in range(dim)]
                for i in range(dim):
                    self.value[i][i] = 1
        
        def show(self):
            for i in range(self.dimx):
                print(self.value[i])
            print(' ')
        
        def __add__(self, other):
            # check if correct dimensions
            if self.dimx != other.dimx or self.dimy != other.dimy:
                raise ValueError("Matrices must be of equal dimensions to add")
            else:
                # add if correct dimensions
                res = matrix([[]])
                res.zero(self.dimx, self.dimy)
                for i in range(self.dimx):
                    for j in range(self.dimy):
                        res.value[i][j] = self.value[i][j] + other.value[i][j]
                return res
        
        def __sub__(self, other):
            # check if correct dimensions
            if self.dimx != other.dimx or self.dimy != other.dimy:
                raise ValueError("Matrices must be of equal dimensions to subtract")
            else:
                # subtract if correct dimensions
                res = matrix([[]])
                res.zero(self.dimx, self.dimy)
                for i in range(self.dimx):
                    for j in range(self.dimy):
                        res.value[i][j] = self.value[i][j] - other.value[i][j]
                return res
        
        def __mul__(self, other):
            # check if correct dimensions
            if self.dimy != other.dimx:
                raise ValueError("Matrices must be m*n and n*p to multiply")
            else:
                # multiply if correct dimensions
                res = matrix([[]])
                res.zero(self.dimx, other.dimy)
                for i in range(self.dimx):
                    for j in range(other.dimy):
                        for k in range(self.dimy):
                            res.value[i][j] += self.value[i][k] * other.value[k][j]
                return res
        
        def transpose(self):
            # compute transpose
            res = matrix([[]])
            res.zero(self.dimy, self.dimx)
            for i in range(self.dimx):
                for j in range(self.dimy):
                    res.value[j][i] = self.value[i][j]
            return res
        
        # Thanks to Ernesto P. Adorio for use of Cholesky and CholeskyInverse functions
        
        def Cholesky(self, ztol=1.0e-5):
            # Computes the upper triangular Cholesky factorization of
            # a positive definite matrix.
            res = matrix([[]])
            res.zero(self.dimx, self.dimx)
            
            for i in range(self.dimx):
                S = sum([(res.value[k][i])**2 for k in range(i)])
                d = self.value[i][i] - S
                if abs(d) < ztol:
                    res.value[i][i] = 0.0
                else:
                    if d < 0.0:
                        raise ValueError("Matrix not positive-definite")
                    res.value[i][i] = sqrt(d)
                for j in range(i+1, self.dimx):
                    S = sum([res.value[k][i] * res.value[k][j] for k in range(self.dimx)])
                    if abs(S) < ztol:
                        S = 0.0
                    try:
                       res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
                    except:
                       raise ValueError("Zero diagonal")
            return res
        
        def CholeskyInverse(self):
            # Computes inverse of matrix given its Cholesky upper Triangular
            # decomposition of matrix.
            res = matrix([[]])
            res.zero(self.dimx, self.dimx)
            
            # Backward step for inverse.
            for j in reversed(range(self.dimx)):
                tjj = self.value[j][j]
                S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
                res.value[j][j] = 1.0/tjj**2 - S/tjj
                for i in reversed(range(j)):
                    res.value[j][i] = res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1, self.dimx)])/self.value[i][i]
            return res
        
        def inverse(self):
            aux = self.Cholesky()
            res = aux.CholeskyInverse()
            return res
        
        def __repr__(self):
            return repr(self.value)

## 2.4.卡尔曼滤波算法为什么叫滤波算法？ ##

以一维卡尔曼滤波为例，如果单纯的相信测量的信号，那么这个信号时包含噪声，很毛糙的，当运行卡尔曼滤波算法做估计时，估计的信号会很光滑，看起来似乎滤掉了噪声的影响，所以称之为卡尔曼滤波算法。

## 2.5 卡尔曼滤波优缺点 ##

优点：在线性问题中被证明是最优估计。计算量小，占用内存小。  
缺点：只能处理连续过程，线性系统。需人为给定系统模型，当系统模型不精确时,滤波效果会有所下降，但可以通过增加采样频率解决此问题。

建议应用场合：输入信号相对平稳或已知被测系统运动学模型，同时要求运算量极小的场合。

[http_www.bzarg.com_p_how-a-kalman-filter-works-in-pictures]: http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
[https_zhuanlan.zhihu.com_p_39912633]: https://zhuanlan.zhihu.com/p/39912633
[1D]: https://img-blog.csdnimg.cn/20200204172648138.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70]: https://img-blog.csdnimg.cn/20200204173212937.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70
[20200204173846977.png]: https://img-blog.csdnimg.cn/20200204173846977.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 1]: https://img-blog.csdnimg.cn/20200204175522932.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 2]: https://img-blog.csdnimg.cn/20200204175740735.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 3]: https://img-blog.csdnimg.cn/20200204180314718.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70
[2020020418134524.png]: https://img-blog.csdnimg.cn/2020020418134524.png
[gaussie]: https://img-blog.csdnimg.cn/20200204181451292.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 4]: https://img-blog.csdnimg.cn/20200204182241797.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 5]: https://img-blog.csdnimg.cn/2020020418231054.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 6]: https://img-blog.csdnimg.cn/20200204183848498.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 7]: https://img-blog.csdnimg.cn/20200204183948767.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 8]: https://img-blog.csdnimg.cn/20200207210630532.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70
[kalman filter]: https://img-blog.csdnimg.cn/20200207171243542.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 9]: https://img-blog.csdnimg.cn/20200207231302667.jpg?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 10]: https://img-blog.csdnimg.cn/20200207231328878.jpg?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 11]: https://img-blog.csdnimg.cn/20200207231347980.jpg?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 12]: https://img-blog.csdnimg.cn/20200207181031630.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 13]: https://img-blog.csdnimg.cn/20200207211907551.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 14]: https://img-blog.csdnimg.cn/20200207212237344.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 15]: https://img-blog.csdnimg.cn/20200207212439412.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 16]: https://img-blog.csdnimg.cn/20200207231551678.jpg?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 17]: https://img-blog.csdnimg.cn/2020020721504651.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh_size_16_color_FFFFFF_t_70 18]: https://img-blog.csdnimg.cn/20200207225110312.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x1dGVyZXNh,size_16,color_FFFFFF,t_70