卡尔曼滤波算法-Kalman filter

一、算法的提出背景：

卡尔曼滤波Kalman filter，"鲁道夫.E.卡尔曼，在一次访问NASA埃姆斯研究中心时，发现这种方法能帮助解决阿波罗计划的轨道预测问题，后来NASA在阿波罗飞船的导航系统中确实也用到了这个滤波器。最终，飞船正确驶向月球，完成了人类历史上的第一次登月"

发布的论文：A New Approach to Linear Filtering and Prediction Problems,

其它滤波方式：

1、有用信号和噪声频谱无交叠

传统滤波：通过通带截止频率选择过滤噪声干扰，例如IIR、FIR

2、信号和噪声频谱有交叠

滤波问题演变为在已知观测量的前提下，对未知信号进行最佳估计的问题。

1）没有估计量的任何先验知识

   经典估计理论，按照估计最佳的评价准则不同划分：

   最大似然估计（MLE）、矩估计、最佳线性无偏估计（BLUE）、最小二乘估计（LSE）

2）有估计量的先验知识（概率密度函数（PDF）、统计特性）

  最佳评价准则划分：

   最小均方误差估计（MMSE）、最大后验估计（MAP）、线性最小均方误差估计（LMMSE）

维纳滤波：信号和观测过程是平稳随机过程，且已经先验知识（功率谱或自相关函数），估计准则为线性最小均方误差估计，依据过去和当前观测值，估计当前信号值。

 图片引用：[百度百科]

应用场景：

雷达跟踪系统，位置和导航系统，控制系统等，估计目标的速度、位置、加速度状态等信息

基本思想：

递归方法解决离散数据线性滤波，已知信号的动态模型和测量方程，估计准则：线性最小均方误差递推估计，依据前一个估计值和当前观测值，估计当前信号值。

原理公式：

一维卡尔曼滤波方程（不考虑系统噪声）：

计算步骤：

1）初始化

初始化系统状态

初始化状态的不确定性p1,0

2）预测

  状态外推方程计算（State Extrapolation）、协方差外推方程计算（Covariance Extrapolation）

3）更新-卡尔曼增益方程计算（Kalman Gain）

4）更新-协方差更新方程计算（Covariance Update）

5）更新-状态更新方程计算（State Update）

输入变量：测量值zn、测量不确定性rn、之前的系统状态估计 、估计不确定性pn,n-1

输出变量：当前系统状态估计、当前状态估计不确定性pn,n

6）预测

  状态外推方程计算、协方差外推方程计算

在2）-6）步骤之间迭代计算。

简化为测量、更新、预测步骤：

 一维卡尔曼滤波方程（考虑系统噪声）：

参考：

【1】信号处理学习笔记2——维纳滤波器 - CodeAntenna

【2】https://www.its203.com/article/qq_42005540/108371057

【3】卡尔曼滤波器(1) -- 背景知识_Yemiekai的博客-CSDN博客_卡尔曼滤波背景

【4】Kalman Filter Tutorial