卡尔曼滤波 android

卡尔曼滤波原理介绍及算法实现

• 基础概念及理论

• 状态方程和观测方程
• 建立卡尔曼滤波评估方程
• 卡尔曼滤波分类

• 六维力传感器滤波案例

• 建立状态方程和观测方程
• 求激励和误差协方差
• 计算卡尔曼增益
• 卡尔曼滤波表达式

• 算法代码实现

本文将参照教科书的介绍，然后结合机械臂末端六维力传感数据滤波为例，详细介绍其公式推导和代码实现。

基础概念及理论

状态方程和观测方程

在对特定问题卡尔曼滤波时，其状态方程和观测方程我们假设是已知的，对于多维变量，可以建立多维状态方程和观测方程

状态方程和观测方程可以用如下框图表示，以激励为输入，以观测量为输出

建立卡尔曼滤波评估方程

使用卡尔曼滤波器，状态方程和观测方程的系数矩阵为已知量，建立方程如（1-1)所示，在此基础上建立卡尔曼滤波评估方程

评估方程中，只有卡尔曼增益K是未知数，其他的都为已知量，卡尔曼增益是协调估计值与测量值信任度的系数，增益越大，则评估值越接近测量值，反之则越接近评估值。

卡尔曼滤波式（1-2）中有两种表达式，其实就是简单的和并了同类项，但其计算框图有所不同分别如下

卡尔曼滤波的计算流程如下图所示

卡尔曼滤波分类

卡尔曼滤波器，可以实现预测、滤波、平滑或插值三类功能，本文主要介绍的是其滤波的功能，其分类依据如下

六维力传感器滤波案例

应用场景：六维力传感器安装在机械臂末端，采用自适应导纳控制实现机械臂末端恒力控制，即期望末端六维力控制到稳定常值，但是由于机械臂绝对定位精度低、环境误差大等原因，导致六维力采集值波动大，直接加入导纳计算式容易引起发散。所以有必要加入滤波，去除噪音，下文将介绍如何建立卡尔曼滤波器在六维力矩传感器上的应用。

建立状态方程和观测方程

六维力我们一般用F表示，为与上文变量同名，我们取状态方程X=F,则观测方程也是Z=F,加入激励矢量和测量误差矢量后，获得表达式如下

求激励和误差协方差

卡尔曼增益与激励矢量的协方差矩阵、测量误差的协方差矩阵密切相关，所以需要先获得激励和误差的协方差矩阵，本文建设其为对角矩阵，并采用两个系数来衡量其大小

计算卡尔曼增益

卡尔曼滤波表达式中，唯一需要更新计算的只有卡尔曼增益，该增益与测量值无关，所以可以提前计算，当然也可以实时计算，计算增益的表达式如下

卡尔曼增益与状态协方差矩阵是一个自回归计算表达式。

卡尔曼滤波表达式

当上式计算完后，直接带入表达式，获得

算法代码实现

用python实现算法如下所示，六维力传感器的每维相互独立，所以计算按六维计算，但绘图仅绘制了第一维

#!/usr/bin/python
#-*-coding:utf-8-*-
#本文档用于书写卡尔曼滤波器算法
#程序员：陈永*
#版权：哈尔滨工业大学(深圳)
#日期：2020.10.4

import numpy as np
import matplotlib.pyplot as plt

#====建立全状态卡尔曼滤波方程=======#
class KalmanFilter(object):
    def __init__(self):
        pass

    #获取建立状态方程和观测方程的参数
    def get_state_measurement_matrix(self, A, B, C):
        #状态转移矩阵
        self.A = np.mat(A)
        #激励转移矩阵
        self.B = np.mat(B)
        #观测矩阵
        self.C = np.mat(C)

    #获取激励矢量和测量误差矢量的协方差
    def get_cov_matrix(self, R, Q = None):
        #误差协方差矩阵
        self.R = np.mat(R)
        #激励激励协方差矩阵
        self.Q = np.mat(Q)

    #获得转移状态协方差矩阵初值
    def get_state_cov_matrix(self, P0):
        self.P = np.mat(P0)

    #获取状态矢量初值
    def get_state_init(self, x0):
        self.x = np.array(x0)

    #计算卡尔曼增益
    def kalman_gain(self):
        #求取卡尔曼增益
        self.K = self.P*self.C.T*(self.C*self.P*self.C.T + self.R).I

        #更新状态转移协方差矩阵
        self.P = self.P - self.K*self.C*self.P

    #输出估计值
    def out_filter_value(self, z):
        # 计算卡尔曼增益
        self.kalman_gain()
        D = np.array(self.A - self.K*self.C*self.A)

        self.x = np.dot(D, self.x) + np.dot(np.array(self.K), z)
        return self.x

def wrench_filter():
    #从上文卡尔曼滤波建立六维力滤波器
    kalm_filt = KalmanFilter()

    #建立状态方程和观测方程:n=m=p
    n = 6
    I = np.eye(n)
    A = np.copy(I)
    B = 0.1*I
    C = np.copy(I)
    kalm_filt.get_state_measurement_matrix(A, B, C)

    #输入测量误差协方差方程
    R = 0.001*I
    kalm_filt.get_cov_matrix(R)

    #输入状态初值
    x0 = np.zeros(n)
    P0 = np.copy(I)
    kalm_filt.get_state_cov_matrix(P0)
    kalm_filt.get_state_init(x0)

    #获取滤波后的数据：本文制造一组虚拟数据
    num = 1000
    T = 0.01
    t = np.linspace(0, (num - 1)*T, num)
    F = np.zeros([num, n])
    for i in range(n):
        F[:, i] = 10 + 2*np.sin(np.pi * t) + 1*np.random.randn(num)

    #带入滤波器
    F_filt = np.zeros([num, n])
    for i in range(num):
        F_filt[i, :] = kalm_filt.out_filter_value(F[i, :])

    #绘画函数,仅考虑第一维
    plt.figure(1)
    plt.plot(t, F[:, 0], label='Fx', color='b')
    plt.plot(t, F_filt[:, 0], label='Fx_filt', color='r')
    plt.title("Kalman_filter")
    plt.xlabel("t/s")
    plt.ylabel("f/N")
    plt.legend()
    plt.show()

if __name__ == "__main__":
    wrench_filter()

计算结果如下

因为我们状态方程建立：假设下一时刻与当前时刻相同，所以导致观测值趋向一个恒定值。