kalman filter

原理：

从官方解释看的时候很蒙蔽好吧，但是知乎上有一博主的解释让人不得不说好，通俗易懂，https://www.zhihu.com/question/47559783

都说的太学究了，大概是这么一回事:我需要用传感器测量一个数据，想要提高测量精度。
1，如果我有很多个传感器，那怎么办？
答:取多个传感器的平均值
2，如果我的多个传感器精度不一样，那怎么办？
答:对于精度高的传感器，我多相信一点，对于精度低的，我少相信一点，然后综合考虑他们给出的结果，在数学上叫做“加权平均”
3，在实时测量中，我并没有很多传感器，但是我还是想要提高精度，怎么办。
3，在实时测量中，我并没有很多传感器，但是我还是想要提高精度，怎么办。

之前在网上根据公式之类的看原理，感觉陷进去了，不能很通俗的去理解含义

测试代码如下。

# -*- coding: utf-8 -*-
import numpy
import numpy as np
import matplotlib.pyplot as plt
import csv
import pandas as pd


def KalmanFilter(z, n_iter):
    # intial parameters

    sz = (n_iter,)  # size of array

    Q = 0.0004  # process variance
    # allocate space for arrays
    xhat = numpy.zeros(sz)  # a posteri estimate of x
    P = numpy.zeros(sz)  # a posteri error estimate
    xhatminus = numpy.zeros(sz)  # a priori estimate of x
    Pminus = numpy.zeros(sz)  # a priori error estimate
    K = numpy.zeros(sz)  # gain or blending factor

    R = 0.6  # estimate of measurement variance, change to see effect

    # intial guesses
    xhat[0] = z[0]
    # P[0] = 1.0  #### intial value is not good
    P[0] = 0
    A = 1
    H = 1

    for k in range(1, n_iter):
        # time update
        xhatminus[k] = A * xhat[k - 1]  # X(k|k-1) = AX(k-1|k-1) + BU(k) + W(k),A=1,BU(k) = 0, w()
        Pminus[k] = A * P[k - 1] + Q  # P(k|k-1) = AP(k-1|k-1)A' + Q(k) ,A=1

        # measurement update
        K[k] = Pminus[k] / (Pminus[k] + R)  # Kg(k)=P(k|k-1)H'/[HP(k|k-1)H' + R],H=1
        xhat[k] = xhatminus[k] + K[k] * (z[k] - H * xhatminus[k])  # X(k|k) = X(k|k-1) + Kg(k)[Z(k) - HX(k|k-1)], H=1
        P[k] = (1 - K[k] * H) * Pminus[k]  # P(k|k) = (1 - Kg(k)H)P(k|k-1), H=1
    return xhat


data=pd.read_csv("../data_csv/result.csv",header=None)
list=data.values.tolist()
list_2 = [float(x) for item in list for x in item] ##list_2为原始数据
# print(list_2)
A = KalmanFilter(list_2,len(list_2))
# print (A)
plt.figure(1)
plt.plot(list_2)
plt.plot(A)
plt.show()

测试时可以先将Q从小往大调整，将R从大往小调整；先固定一个值去调整另外一个值，看收敛速度与波形输出。
在去年做的项目中聚焦一类的需要电压来控制物镜离焦距离，所以就涉及到了数据的处理方式，最开始的各种滤波都试过，效果一般，就用了kalman，效果还不错。。。

仿真测试结果如下