卡尔曼（kalman）滤波学习测试例

下面两套代码一套是python，一套是matlab，效果是一样的。

PYTHON


import numpy as np
import matplotlib.pyplot as pltt = np.arange(1, 1001)
nsig = 5 * np.sin(0.01 * t) + np.random.rand(len(t)) + np.random.randn(len(t)) + 5 * np.cos(0.05 * t + np.pi/1.5)
kf = np.zeros_like(nsig)x = 0  # state, x(k) = a * x(k-1) + b * u(k)
a = 1  # state transfer matrix
b = 0  # control matrix
h = 1  # observer matrix
p = 0  # estimate cov
q = 0.002  # process cov
r = 0.05  # observer cov
g = 0  # kalman gaingain = np.zeros_like(nsig)for i in range(len(nsig)):x = a * x + b * nsig[i]p = a * p * a + qg = p * h / (h * p * h + r)x = x + g * (nsig[i] - h * x)p = (1 - g * h) * pkf[i] = xgain[i] = gplt.plot(t, nsig, label="noise")
plt.plot(t, kf, label="filtered")
plt.plot(t, gain, label="kalman gain")
plt.legend()
plt.grid(which="minor")
plt.show()

MATLAB

t = 1 : 1000;
nsig = 5 * sin(0.01 * t) + rand(1, length(t)) + randn(1, length(t)) + 5 * cos(0.05 * t + pi/1.5);
kf = zeros(size(nsig));x = 0; % state, x(k) = a * x(k-1) + b * u(k)
a = 1; % state transfer matrix
b = 0; % control matrix
h = 1; % observer matrix
p = 0; % estimate cov
q = 0.002; % process cov
r = 0.05; % observer cov
g = 0; % kalman gaingain =zeros(size(nsig));for i = 1 : length(nsig)x = a * x + b * nsig(i);p = a * p * a + q;g = p * h /(h * p * h + r);x = x + g * (nsig(i) - h * x);p = (1 - g * h) * p;kf(i) = x;gain(i) = g;
endplot(t, nsig, t, kf, t, gain);
legend("noise", "filtered", "kalman gain");
grid minor;