滤波算法

卡尔曼(Kalman)滤波算法直观理解  https://zhuanlan.zhihu.com/p/77327349

Allan方差分析   https://wenku.baidu.com/view/a4e03adc6f1aff00bed51efc.html

- - 算出 估计值和测量值的误差 

 

示例：

kalman滤波，根据GPS位置信息和IMU速度信息，预估导弹位置；

#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
@File    : kalman_filter.py
@Time    : 2021/1/5 10:28
@Email   : todo@todo.todo
@Software: PyCharm
@desc    :
使用kalman filter
- 测量数据方差，一般由Allan方差分析得到；
- 预估数据方差，可由多个位置、速度等测量数据的方差，根据预估模型计算得到，迭代更新；

"""

import numpy as np

# 模拟数据
t = np.linspace(1, 100, 100)
a = 0.5
position = (a * t ** 2) / 2

position_noise = position + np.random.normal(0, 120, size=(t.shape[0]))  # 模拟GPS测量的位置
import matplotlib.pyplot as plt

plt.plot(t, position, label='truth position')
plt.plot(t, position_noise, label='only use measured position')

# 初试的估计导弹的位置就直接用GPS测量的位置
predicts = [position_noise[0]]
position_predict = predicts[0]

predict_var = 0  # 预测数据的方差
odo_var = 120 ** 2  # 这是我们自己设定的位置测量仪器的方差，越大则测量值占比越低
v_std = 50  # 速度测量仪器的方差

for i in range(1, t.shape[0]):
    dv = (position[i] - position[i - 1]) + np.random.normal(0, 50)  # 模拟 从IMU读取出的速度
    position_predict = position_predict + dv  # 利用上个时刻的位置和速度 预测当前位置
    predict_var += v_std ** 2  # 更新 预测数据的方差
    # 下面是Kalman滤波
    position_predict = position_predict * odo_var / (predict_var + odo_var) + position_noise[i] * predict_var / (
                predict_var + odo_var)
    predict_var = (predict_var * odo_var) / (predict_var + odo_var) ** 2
    predicts.append(position_predict)

plt.plot(t, predicts, label='kalman filtered position')

plt.legend()
plt.show()

 

卡尔曼(Kalman)滤波算法原理、C语言实现及实际应用

https://blog.csdn.net/CSDN_X_W/article/details/90289021?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-1.nonecase&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-1.nonecase

 

理解与推导贝叶斯滤波(Bayes Filter)算法  https://zhuanlan.zhihu.com/p/75880143

 

 

十大滤波算法程序

https://blog.csdn.net/Aaron_Suen/article/details/83340956?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-1.nonecase&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-1.nonecase