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

基础概念及理论状态方程和观测方程建立卡尔曼滤波评估方程卡尔曼滤波分类 六维力传感器滤波案例建立状态方程和观测方程求激励和误差协方差计算卡尔曼增益卡尔曼滤波表达式 算法代码实现 本文将参照教科书的介绍，然后结合机械臂末端六维力传感数据滤波为例，详细介绍其公式推导和代码实现。

基础概念及理论

状态方程和观测方程

在对特定问题卡尔曼滤波时，其状态方程和观测方程我们假设是已知的，对于多维变量，可以建立多维状态方程和观测方程 状态方程和观测方程可以用如下框图表示，以激励为输入，以观测量为输出

建立卡尔曼滤波评估方程

使用卡尔曼滤波器，状态方程和观测方程的系数矩阵为已知量，建立方程如（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()

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