第一个卡尔曼滤波程序

卡尔曼滤波：主要分为预测、观测、更新三个过程；

预测：起始状态和运动方程的高斯方程相加

观测：设备测量的得到的值

更新:根据预测和观测得到的结果，更新得到新的结果.(卡尔曼滤波)

# https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/tree/v2.0
import random

import numpy as np
from numpy.random import randn
from math import sqrt
import matplotlib.pyplot as plt
import math
from scipy.linalg import inv

random.seed(10)
# 卡尔曼滤波的本质：
# 预测的时候是高斯相加;
# 更新的时候是高斯相乘;
def print_result(predict,update,z,epoch):
    predict_template = '{:3.0f} {:7.3f} {:8.3f}'
    update_template  = '\t {:.3f}\t{:7.3f}\t{:7.3f}'
    print(predict_template.format(epoch,predict[0],predict[1]),end='\t')
    print(update_template.format(z,update[0],update[1]))

def plot_result(epochs,prior_list,x_list,z_list):
    epoch_list = np.arange(epochs)
    plt.plot(epoch_list,prior_list,linestyle=':',color='r',label='prior/predicted_pos',lw=2)
    plt.plot(epoch_list,x_list,linestyle='-',color='g',label='posterior/updated_pos',lw=2)
    plt.plot(epoch_list,z_list,linestyle=':',color='b',label='likelihood/measurement',lw=2)
    plt.legend(loc='center left',bbox_to_anchor=(1,0.5))
    plt.show()


from collections import namedtuple
gaussian = namedtuple("Gaussian",["mean","var"])
gaussian.__repr__ = lambda s: f"N{s[0]:.3f},var={s[1]:.3f}"



def kalman_predict(posterior,movement):
    # 上一轮的后验，其实就是新一轮的一个predict前的分布
    x, P = posterior # 取出起点的均值和方差
    dx, Q = movement # 取出位移的速度和方差

    # prediction
    x = x + dx
    P = P + Q
    return gaussian(x,P)


# 高斯分布下的运动观测:多个数据源，加权平均后确定，比单独相信两个数据源更准确;
# 观测:传感器告诉你位置
# 预测：自己根据经验推算的公式,先验
# 更新/更新后的位置/矫正后的位置,后验分布
def gaussian_multiply(g1,g2):
    mean  = (g1.var*g2.mean+g2.var*g1.mean)/(g1.var+g2.var)
    variance = (g1.var*g2.var)/(g1.var+g2.var)
    # 返回两个告诉的交集
    return gaussian(mean,variance)

def update_kalman(measurement, prior):
    x, P = prior
    z, R = measurement

    y = z-x
    K = P/(P+R)

    x = x + K*y
    P = (1-K)*P
    return gaussian(x,P)

# 第一个卡尔曼滤波
def first_kalman_norm():
    # 1.一些初始的状态和变量的赋值
    motion_var = 1.0 # 人的运动方差
    sensor_var = 2.0 # GPS的运动方差
    x = gaussian(0,20**2)  # 人的初始位置
    velocity = 1.0
    dt = 1 # 时间单位的最小刻度
    motion_model = gaussian(velocity, motion_var) # 运动模型(过程模型)

    # 2.生成数据
    zs = [] # 记录观测数据
    current_x = x.mean
    for _ in range(10):# 我们让小明走10s,每走1s就看看GPS，并且把这些数据存起来
        # 2.1 先生成我们的运动数据
        v = velocity + randn()*motion_var # 速度会有波动
        current_x += v*dt
        # 2.2生成的观测数据
        measurement = current_x+randn()*sensor_var #gps观测也有一定的误差
        zs.append(measurement) #

    prior_list,x_list,z_list = [], [], []
    print('epoch\tPREDICT\t\t\tUPDATE')
    print('  \t\tx      var\t\t          z\t    x     var')
    for epoch,z in enumerate(zs):
        prior = kalman_predict(x,motion_model) # 运动预测,预测1s后的位置 # 两个高斯之和
        likelihood = gaussian(z,sensor_var) # 观测1s后的数据

        x = update_kalman(likelihood,prior) # 结合观测 # 两个高斯的交集

        # 预测的均值   方差       观测值   卡尔曼均值  方差
        print_result(prior,x,z,epoch)
        prior_list.append(prior.mean)
        x_list.append(x.mean)
        z_list.append(z)

    print()
    print(f"final estimate:       {x.mean:30.3f}") # 经过卡尔曼滤波得到的值
    print(f"actual final estimate:{current_x:30.3f}") #真实的值
    plot_result(10,prior_list,x_list,z_list)
    print('finish')

if __name__ == "__main__":
    first_kalman_norm()