Loading

卡尔曼滤波 详细推导/核心思想

Recursive bayesian estimation

Recursive Bayesian estimation, also known as a Bayes filter, is a general probabilistic approach for estimating an unknown probability density function (PDF) recursively over time using incoming measurements and a mathematical process model.

wiki

Kalman Filter

优越性

excerpt from wiki

Kalman filtering is based on linear dynamical systems discretized in the time domain. They are modeled on a Markov chain built on linear operators perturbed by errors that may include Gaussian noise.

Optimality of Kalman filtering assumes that errors have a normal (Gaussian) distribution.

  • 噪声信号服从零均值高斯分布
  • 各自独立

In the words of Rudolf E. Kálmán: "In summary, the following assumptions are made about random processes: Physical random phenomena may be thought of as due to primary random sources exciting dynamic systems. The primary sources are assumed to be independent gaussian random processes with zero mean; the dynamic systems will be linear."[9]

Though regardless of Gaussianity, if the process and measurement covariances are known, the Kalman filter is the best possible linear estimator in the minimum mean-square-error sense.[10]

特点

  • Suitible for continuously changing linear system
  • Light on memory
    • 只需要保存状态state即可，不需要记录历史信息

一个例子：机器人

the state in Kalman Filter can be anything that may exist in a linear dynamic system.

State

这个机器人有一个精度为10米的GPS,可用但不够精确。

我们的目标是达到尽可能高的准确度

Prediction

机器人知道控制车轮速度的指令。然而由于地形起伏等未知环境，它无法准确预测其位置。

  • GPS censor: State
    • 不精确的测量值
  • Control Commands: Prediction
    • 不完全可信的估计值
  • both with inaccuracy/uncertainty

Kalman filter can achieve more accurate estimating than measurement and prediction do.

KF可以通过迭代式的后验推断获得更精确的值（本质上也是一个预测值）

卡尔曼滤波器的工作过程

基本概念

from wiki

两个步骤迭代进行

  • 基于先验知识的预测步
  • 对比预测结果和观测结果的更新步

实际的状态量是通过两步计算得到的（两个步骤中都出现了x和P的更新）

Wiki KF的算法流程

  • State/状态量 \(\mathbf{x}_{t}\)
    • 状态\(\mathbf{x}_{t} \sim \mathcal{N}(\hat{\mathbf{x}}_{t},\mathbf{P}_{t})\)
    • 状态根据\(\mathbf{F}_{t}\)线性转换
    • 可能有已知外力(控制)\(\mathbf{u}_{t}\)，经过\(\mathbf{B}_{t}\)作用于当前状态
    • 叠加过程噪声\(\mathbf{w}_{t} \sim \mathcal{N}(0,\mathbf{Q}_{t})\)
  • Observation/观测量
    • 观测量是状态量经由观测模型\(\mathbf{H}_{t}\)转换得到
    • 叠加观测噪声\(\mathbf{v}_{t} \sim \mathcal{N}(0,\mathbf{R}_{t})\)
  • 起始状态\(\mathbf{x}_{0}\)，各个时间步的过程噪声以及观测噪声都是独立的

可以概括为

  • Morkov Chain
  • Linear Operators
  • Gaussian Noise

卡尔曼滤波器的六大前提假设
知乎

状态量的分布

首先我们有一些需要估计的量。

这些量可以任意取值，但是总有某些取值拥有更高的可能性

独立变量

相关变量

- 假设1: 状态量服从正态分布

\[\mu_{t} = \hat{\mathbf{x}}_{t} \in \R^{d \times 1},\mathbf{P}_{t} \in \R^{d \times d} \]

我们假定某一时刻的状态值

\[\mathbf{x}_{t} \sim \mathcal{N}(\hat{\mathbf{x}}_{t},\mathbf{P}_{t}) \]

在参考资料当中时间用\(k\)表示，协方差表示为\(\mathbf{P}_{k}\)

在\(t-1\)时刻，系统拥有一个当前状态\(\hat{\mathbf{x}}_{t-1}\)

记住，我们所拥有的永远都是状态的最优估计\(\hat{\mathbf{x}}_{t}\)，现实当中没有真正客观的状态

+ Question: 如何初始化？

初值似乎对卡尔曼滤波影响不大，但是有error propagation的可能性存在

可参见NTU

基于先验知识的状态转移

It takes every point in our original estimate and moves it to a new predicted location, which is where the system would move if that original estimate was the right one.

在理想情况下，利用先验知识构造一个状态转移矩阵

例如针对速度和位置，如果假定匀速运动，利用基本运动学方程，写出矩阵化形式

状态转移矩阵在改变测量值的同时也会改变不同维度之间的协方差矩阵

线性变换改变了协方差矩阵

引入可知干预(Known External Forces)

Everything is fine if the state evolves based on its own properties.

Everything is still fine if the state evolves based on external forces, so long as we know what those external forces are.

比如在运动模型中引入一个已知的加速度

未知外力/过程噪声(Process Noise)

Process Noise from wikipedia

比如这个机器人，轮子可能打滑，或者一个飞机受到风力的影响。即使在已知外力和已知作用规律的情况下，对于结果的预测仍然可能出现偏差

调整后的预测 = 原始预测+噪声

一个高斯分布会在噪声的作用下变成一个新的高斯分布

独立高斯分布的可加性

把噪声看做\(\mathbf{Q}_{t}\sim \mathcal{N}(0,\sigma^{2})\)

\(\hat{\mathbf{x}}_{t} = \hat{\mathbf{x}}_{t} + \mathbf{Q}_{t}\)

+ Question: 为什么噪声没有加在x上？

想要在x上加噪声，本意应该是刻画高斯分布中心（均值的）不确定性

\[\begin{align*} \color{red}{\mathbf{\hat{x}}_t} &= \mathbf{F}_t {\color{red}{\mathbf{\hat{x}}}_{t-1}} + \mathbf{B}_t {\vec{\mathbf{u}_t}} + \mathbf{Q}'_{t}\end{align*}\]

\[\mathbf{Q}'_{t}\sim \mathcal{N}(0,\sigma_{\mu}^{2}) \]

在先验规律和已知外力的作用下，可以得到分布中心 \(\hat{{\mathbf{x}}}_{t} = \mathbf{F}_t {\mathbf{\hat{x}}_{t-1}} + \mathbf{B}_t {\vec{\mathbf{u}_t}}\)

而如果在这个中心点上添加一个零均值高斯噪声\(\mathbf{Q}'_{t}\)，那么这个中心点将会从一个向量变成一个依概率分布的向量，这并不是我们定义\(\hat{{\mathbf{x}}}\)的本意

实际上，应该是

\[\begin{align*} \color{green}{\mathbf{x}_t} &= \mathbf{F}_t {\color{green}{\mathbf{x}_{t-1}}} + \mathbf{B}_t {\vec{\mathbf{u}_t}} + \mathbf{Q}'_{t}\end{align*}\]

噪声是作用在分布上的

[Note💡]: 所以噪声就是加在x上的，但是要注意分清楚\(\hat{\mathbf{x}}_{t}\)这个分布 中心 以及\(\mathbf{x}_{t}\)这个高斯状态分布本身

预计测量值/Predicted Measurement

传感器的作用是把状态转换为测量值，这一步用\(\mathbf{H}_{t}\)进行刻画

tells us something indirect about the state— in other words, the sensors operate on a state and produce a set of readings.

"reading"被抽象为作用在状态上的线性变换

From each reading we observe, we might guess that our system was in a particular state. But because there is uncertainty, some states are more likely than others to have produced the reading we saw

实际测量值/Observed Measurement

和噪声下的状态转移（预测步）不同的是，观测过程有一个实际测量值(传感器读数) \(\hat{\mathbf{z}}_{t}\)。在以其为中心的基础上，我们引入观测噪声\(\mathbf{R}_{t }\)

在实际测量值叠加观测噪声，让其变成一个分布

\[\mathbf{z}_{t} \sim \mathcal{N}(\hat{\mathbf{z}}_{t},\mathbf{R}_{t}) \]

更新步的矩阵形式

至此，我们获得了两个分布

  • 预计测量值

\[\mathbf{\mathbf{x}}_{t} \sim \mathcal{N}(\mathbf{H}_t {\hat{\mathbf{x}}}_t, \mathbf{H}_t {\mathbf{P}_t} \mathbf{H}_t^T) \]

  • 实际测量值

\[\mathbf{z}_{t} \sim \mathcal{N}(\hat{\mathbf{z}}_{t},\mathbf{R}_{t}) \]

实际测量值以及预计测量值是两个不同的分布

将两个分布相乘，相当于取交集。相乘之后可以看到分布变得更加集中，也就是说我们通过计算得出的联合分布利用了预计测量值以及实际测量值，折中了两者，更为精确

两个单变量高斯分布的乘积，注意需要归一化

两个多维高斯分布的乘积，注意需要归一化

ref: stanford, matrix-cookbook

K为卡尔曼增益

将两者相乘

做两个高斯分布的乘积

化简

这便是最终的更新形式

全过程

注意，非常有名的博客和Wiki里面推导出来的形式乍看上去是不一样的。但实际上两者完全等价，在博客当中没有innovation(新息)这个概念

  • Predict:
    • 多维高斯分布的性质
    • \(\hat{\mathbf{x}}_{k} := \text{Predict}(\hat{\mathbf{x}}_{k-1})\)
  • Update:
    • 预测分布和观测分布分布的乘积
    • \(\hat{\mathbf{x}}_{k}:=\hat{\mathbf{x}}_{k}' :=\text{Update}(\hat{\mathbf{x}}_{k})\)

(博客里面的\(\hat{\mathbf{x}}_{k}\)就是指的\(\text{mean}(\mathbf{x}_{k}) = \mathbb{E}(\mathcal{N}(\hat{\mathbf{x}}_{k},\mathbf{P}))\)

Alpha beta filter

https://en.wikipedia.org/wiki/Alpha_beta_filter

Extended KF

https://github.com/balzer82/Kalman#extended-kalman-filter

KF

Extended KF

ref

posted @ 2022-04-24 10:35  ZXYFrank  阅读(297)  评论(0)  编辑  收藏  举报