在对特定问题卡尔曼滤波时，其状态方程和观测方程我们假设是已知的，对于多维变量，可以建立多维状态方程和观测方程
状态方程和观测方程可以用如下框图表示，以激励为输入，以观测量为输出
使用卡尔曼滤波器，状态方程和观测方程的系数矩阵为已知量，建立方程如（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()
计算结果如下
因为我们状态方程建立：假设下一时刻与当前时刻相同，所以导致观测值趋向一个恒定值。