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

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

PYTHON

import numpy as np
import matplotlib.pyplot as plt

t = 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 gain

gain = np.zeros_like(nsig)

for i in range(len(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

plt.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 gain

gain =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;
end

plot(t, nsig, t, kf, t, gain);
legend("noise", "filtered", "kalman gain");
grid minor;

参考文献