卡尔曼滤波算法

最近在做数据分析，用到了卡尔曼滤波，对里面的公式有些不理解，学习之后，现在做个总结，卡尔曼滤波本质就是用观测值和测量值进行数据估计，用上一次的最优值来估计本次的最优值，是线性滤波系统。
五大公式：

针对五大公式的理解，参考了https://blog.csdn.net/tiandijun/article/details/72469471
但是这篇文章举得例子不是很好，里面用到的是matlab编写的代码。现在列出python的代码;

#import module
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt

N=200
w=np.random.randn(1,N)
# w[0][0] = 0
print (w)
a=1;
x = [0] * N
x[0]=25
real = [0] * N
real[0]=25
# w为过程噪声， x为估计值
for k in range(1,N):  #循环时i等于1到N-1
    # print (k)
    x[k] = a*x[k-1] + w[0][k-1]
    real[k]=a*real[k-1]
V=np.random.randn(1,N) #V为观察噪声
q1=np.std(V[0],ddof=1)
Rvv=np.dot(q1,q1);
q2=np.std(x,ddof=1)
Rxx=np.dot(q2,q2);
q3=np.std(w[0],ddof=1)
Rww=np.dot(q3,q3);
#Y为测量值
c=0.8;
Y = [0] * N
for i in range(0,N):
    Y[i]=c*x[0]+V[0][i]*2;
p = [0] * N#Pk'
p1 = [0] * N#Pk
s = [0] * N#最优值
b = [0] * N#增益K
p[0] =0;
s[0] =0;
for t in range(1,N):
    p1[t]=np.dot(a,a)*p[t-1]+Rww;     #p1为方程中p'
    b[t]=c*p1[t]/(np.dot(c,c)*p1[t]+Rvv);    
    s[t]=a*s[t-1]+b[t]*(Y[t]-a*c*s[t-1]);    
    p[t]=p1[t]-c*b[t]*p1[t];

tt = [0] * N
for i in range(0,N):   
    tt[i] = i+1;
plt.plot(tt, real, 'k', tt,s,'r',tt,Y,'g',tt,x,'b');
plt.xlabel('time')
plt.ylabel('temperature')
plt.title('temperature change chart')
label = ["real", "optimized", "measure", "watch"]
plt.legend(label, loc = 0, ncol = 2)
plt.show()

为了更好的理解卡尔曼滤波算法，我在视频上看到另外一个讲解，https://www.iqiyi.com/w_19rsofi8ct.html
里面例子非常好，是用的汽车运动位移和速度，这个具有一般性。matlab代码如下

T=(1:100);
Z=(1:100);
noise=randn(1,100);
Z=Z +noise;

X=[0;0];
P=[1 0; 0 1];
F=[1 1; 0 1];
Q=[0.0001 0 ; 0 0.0001];
H=[1 0];
R=1;
figure;
hold on;
for i = 1:100
	X_=F*X;
	P_=F*P*F'+Q;
	K=P_*H'/(H*P_*H'+R);
	X=X_+K*(Z(i)-H*X_);
	P=(eye(2)-K*H)*P_;
	plot(T(i), X(1),‘r’, T(i), X(2),‘g’)
end