# 结论

1 在匀速运动的状态方程中，协方差矩阵Q表示随机的加速度。

2 此值不能太小，否则稍许扰动就会导致新息的统计距离偏大，甚至超出卡方分布的置信度门限。

# 代码

```import sys
import traceback
import getopt
import numpy as np
import matplotlib.pyplot as plt

path = 'KalmanEstimateConstantV1D/'

# 模拟观测数据
def simData(x0, v, T, sigma, N):
truth = np.zeros(N)
truth[0] = x0
truth[1:] = v * T
truth = np.cumsum(truth)  # 真实坐标
w = np.random.normal(0, sigma, N)  # 添加观测噪声
print('mean = ', np.mean(w))
print('std = ', np.std(w, ddof = 1))
data = truth + w
# np.savetxt(path + "SimData.txt", data)  # 数据存盘
return data, truth

# 1维数据滤波
def filter1DconstantV(z, q, r, T):
xkk = np.zeros((2, z.size))  # 滤波值，状态向量为[x, vx]'
pkk = np.zeros((2, 2*z.size))  # 滤波协方差，2x2矩阵
dis = np.zeros(z.size)  # 新息统计距离

# 状态转移矩阵
F = [ [1, T],
[0, 1]]
Ft = np.transpose(F)  # F的转置
# 协方差Q
gama = np.array([[T**2 / 2], [T]])
Q = q * gama @ gama.T

# 观测矩阵
# H = np.array([1, 0])    # H.shape is (2,) 涉及到矩阵计算，此版本矩阵转置不对，不能使用
H = np.array([1, 0]).reshape(1, 2)    # H.shape is (1, 2)
Ht = np.transpose(H)  # H的转置
# 观测误差
R = r

# 初始化
xkk[:, 0] = [z[0], 0]
xkk[:, 1] = [z[1], (z[1] - z[0])/T]
pkk[:, 2:4] = [[r, r/T],
[r/T, 2*r / T**2]]

# 依次滤波
for i in range(2, z.size):
xki = F @ xkk[:, i-1]  # 状态的一步预测
pki = F @ pkk[:, 2*(i-1):2*i] @ Ft + Q  # 协方差的一步预测
zki = H @ xki  # 量测的预测
S = H @ pki @ Ht + R  # 新息的协方差，此例中S为标量
v = z[i] - zki  # 新息
if abs(S) < 1e-3 :  # S不可逆
print('|S| < 1e-3')
K = 0  # 增益
dis[i] = -1  # 新息统计距离，异常值
else:
invS = 1 / S
K = pki @ Ht * invS  # 增益
dis[i] = v * invS * v  # 新息统计距离

xkk[:, i] = (xki.reshape(2, 1) + K * v).reshape(2,)  # 状态的更新
pkk[:, 2*i : 2*(i+1)] = pki - S * K @ np.transpose(K)  # 协方差的更新
# pkk[:, 2*i : 2*(i+1)] = (np.eye(2) - K @ H) @ pki  # 协方差的更新
if np.min(np.linalg.eigvals(pkk[:, 2*i : 2*(i+1)])) < 1e-3 :
print(f'i = {i}, pkk不正定')

return xkk, pkk, dis

# 画图
# 输入：
#       truth, 真值
#       z, 观测值
#       T，采样间隔
#       rs, 不同参数组R
#       qs, 不同参数组Q
#       xkk, 各组参数对应的滤波值
#       pkk, 各组参数对应的滤波协方差矩阵
#       dis，各组参数对应的统计距离
#       saveFig，是否保存图片
def display(truth, z, T, rs, qs, xkk, pkk, dis, saveFig):
N = truth.size

# 颜色表
colors = ('b', 'g', 'r', 'y', 'c',  # 参数1 - 5
'm',  # 观测值用
'orange')  # 真值用

# markers
markers = ('o', 'v', 'd', 'x', '+')  # 参数1 - 5

# 画 真值、观测值、滤波值
title = 'x轴坐标1'
plt.figure(title)
plt.plot(np.arange(N) + 1, truth, color = colors[len(colors) - 1], label = '真值')  # plot
plt.plot(np.arange(N) + 1, z, '.-', color = colors[len(colors) - 2], label = '观测值')  # plot
for row in range(0, len(rs)):
plt.plot(np.arange(N) + 1, xkk[2*row, :], 'o-',
marker = markers[row], color = colors[row], label = f'({row + 1}):Q = {qs[row]: .6f}, R = {rs[row]:.4f}')  # plot

plt.legend()
plt.xlabel('t(s)')  # 设置x轴标签
plt.ylabel('x(m)')  # 设置y轴标签
plt.grid()
if saveFig:
plt.savefig(path + title)

# 画速度
title = '速度2'
plt.figure(title)
plt.plot(np.arange(N-1) + 1, np.diff(truth[:2]) * np.ones(N-1) / T, color = colors[len(colors) - 1], label = '真值')  # plot
plt.plot(np.arange(N-1) + 1, np.diff(z) * (1/T), '.-', color = colors[len(colors) - 2], label = '观测值')  # plot
for row in range(0, len(rs)):
plt.plot(np.arange(N-1) + 1, xkk[2*row+1, 1:], 'o-',
marker = markers[row], color = colors[row], label = f'({row + 1}):Q = {qs[row]: .6f}, R = {rs[row]:.4f}')  # plot

plt.legend()
plt.xlabel('t(s)')  # 设置x轴标签
plt.ylabel('v(m/s)')  # 设置y轴标签
plt.grid()
if saveFig:
plt.savefig(path + title)

# 画 滤波值的方差
title = 'x滤波值的方差3'
fig = plt.figure(title)
for row in range(0, len(rs)):
plt.plot(np.arange(N-1) + 1, pkk[2*row, 2::2], 'o-',
marker = markers[row], color = colors[row], label = f'({row + 1}):Q = {qs[row]: .6f}, R = {rs[row]:.4f}')  # plot

plt.legend()
plt.xlabel('t(s)')  # 设置x轴标签
plt.ylabel('pkk_x')  # 设置y轴标签
plt.grid()
if saveFig:
plt.savefig(path + title)

# 画 滤波值的方差
title = 'v滤波值的方差4'
fig = plt.figure(title)
for row in range(0, len(rs)):
plt.plot(np.arange(N-1) + 1, pkk[2*row+1, 3::2], 'o-',
marker = markers[row], color = colors[row], label = f'({row + 1}):Q = {qs[row]: .6f}, R = {rs[row]:.4f}')  # plot

plt.legend()
plt.xlabel('t(s)')  # 设置x轴标签
plt.ylabel('pkk_v')  # 设置y轴标签
plt.grid()
if saveFig:
plt.savefig(path + title)

# 画 新息的统计距离
title = '新息的统计距离5'
fig = plt.figure(title)
for row in range(0, len(rs)):
plt.plot(np.arange(N-2) + 1, dis[row, 2:], 'o-',
marker = markers[row], color = colors[row], label = f'({row + 1}):Q = {qs[row]: .6f}, R = {rs[row]:.4f}')  # plot

plt.legend()
plt.xlabel('t(s)')  # 设置x轴标签
plt.ylabel('统计距离')  # 设置y轴标签
plt.grid()
if saveFig:
plt.savefig(path + title)

plt.show()

# Works 实现主要功能
def Works():
saveFig = True  # 保存图片
# saveFig = False  # 不保存图片

x0 = 0  # 初始值
v = 5  # 速度，单位：1m/s
T = 2  # 观测周期，单位：1s
r = 1e-0  # 观测值标准差，单位：1m
N = 50  # 观测次数
z, truth = simData(x0, v, T, r, N)  # 模拟观测数据
# plt.figure("模拟数据")
# plt.plot(z, '.-')

q = 8 * (r**2) / (T**4)  # 根据两点间匀加速运动推导得到加速度的标称值
# 只能说q不超过该标称值，但不能将q设为该值
# 对于匀速运动，q表示加速度，应充分小，符合状态方程是依据匀速运动变化的假设
q *= 0.01

# 五组参数
rs = [r,    r,     r,      r,    r]
qs = [q/4,  q/2,   q,    2*q,  4*q]
xkk = np.zeros((2*len(rs), z.size))  # 滤波值，每2行依次对应一组参数
pkk = np.zeros((2*len(rs), 2*z.size))  # 滤波协方差，每2行对应一组参数，每2列对应一次滤波
dis = np.zeros((len(rs), z.size))  # 新息统计距离，各行依次对应一组参数

for idxR in range(0, len(rs)):
r = rs[idxR]
q = qs[idxR]
xkk[2*idxR:(2*idxR+2), :], pkk[2*idxR:(2*idxR+2), :], dis[idxR, :] = filter1DconstantV(z, q, r, T)  # 滤波

# 画图
display(truth, z, T, rs, qs, xkk, pkk, dis, saveFig)

return 0

# 异常处理函数
class Usage(Exception):
def __init__(self, msg):
self.msg = msg

# main()函数
def main(argv=None):
if argv is None:
argv = sys.argv
try:
try:
opts, args = getopt.getopt(argv[1:], "h", ["help"])
except(getopt.error, msg):
raise (Usage(msg))

# 使plt支持中文
plt.rcParams['font.sans-serif'] = ['SimHei']
plt.rcParams['axes.unicode_minus'] = False

# Works 实现主要功能
Works()
except Usage as err:  # 待处理新增代码中的异常
print(sys.stderr, err.msg)
print(sys.stderr, "for help use --help")
return 1
except Exception as e:
print("print_exception()")
exc_type, exc_value, exc_tb = sys.exc_info()
print('the exc type is:', exc_type)
print('the exc value is:', exc_value)
print('the exc tb is:', exc_tb)
traceback.print_exception(exc_type, exc_value, exc_tb)
finally:
print('exit main')

# 直接运行的入口
if "__main__" == __name__:
sys.exit(main())```

0 条评论

• ### 1，Kalman滤波器参数如何选取

新冠居家封闭期间，对参考文献中估计常数的例子，初次使用python的NumPy库进行仿真，深入理解Kalman滤波器的参数对滤波性能的影响。

• ### Python可视化学习（饼状图,坐标系...）

今天资源君带大家学习一下Python的可视化，何谓可视化呢？我们常常听说Python的数据分析，数据分析中很重要的一个就是将数据展示出来，如何展示出来呢...

• ### 深度学习数学基础一--最小二乘法

之前总是先上手一些比较高级的神经网络算法，CNN，RNN等。可是总觉得有些知识原理总是羁绊着我进一步理解。这才意识到基础的重要性。所以，就一点一点的从基础数学最...

• ### matplot 同时绘制多个图形(一）

matplotlib.pyplot中的subplot()函数可以用来在一张画布上绘制多个图形。

• ### 零基础学编程042：画函数图像

孩子马上就要参加高考了，我以前还能帮着辅导一下数学功课，现在就不行了，一来她很忙，晚上很晚才到家，二来高中的数学题太变态，琢磨一个小时可能也解不出一道。 前几天...

• ### Caffe2 - (八)图像加载与预处理

Caffe 使用的是 OpenCV 的 Blue-Green-Red (BGR)，而不是通用的 Red-Green-Blue (RGB).

• ### Matplotlib新手上路(中)

接上回继续 一、多张图布局(subplot) 1.1 subplot布局方式 import matplotlib.pyplot as plt plt.figu...