kalman filter java_Kalman filters（一）

3.1理论推导

首先假设状态变量为x，观测量为z，那么结合贝叶斯后验概率模型：

$posterior = \frac{{likelihood \times prior}}{{normalization}} \Leftrightarrow P\left( {\left. x \right|z} \right) = \frac{{P\left( {\left. z \right|x} \right)P\left( x \right)}}{{P\left( z \right)}}\\$

多目标跟踪从形式上讲可以理解为最大化后验概率，现在结合第二节的内容，假设状态变量x服从高斯分布，反映的是运动模型的不稳定性。基于状态变量x的估计先验，观测量z也服从高斯分布，反映的是量测误差，比如传感器误差。那么我们就可以利用高斯分布的融合来刻画Kalman滤波器的更新部分。

这里我们先给出一阶Kalman滤波器的公式，其中预测环节就是基于线性运动特性对状态变量的预测，即：

$\begin{array}{l} x = Fx + Bu\\ P = FxF^T + Q \end{array}\\$

其中

$x$ 为状态变量的均值，

$P$ 为预测方差，那么对应的高斯分布方差即为

$FPF^T$ ，而

$Q$ 则是线性运动模型本身的误差，由此得到预测环节。即预测结果服从高斯分布

$N(x,P)$ 。

对于更新环节，同样地，假设量测误差分布满足

$N(z,R)$ ，那么：

$\begin{array}{l} \mu = \frac{{{\mu _z}{\sigma _x}^2 + {\mu _x}{\sigma _z}^2}}{{{\sigma _x}^2 + {\sigma _z}^2}} = \frac{{{\sigma _x}^2}}{{{\sigma _x}^2 + {\sigma _z}^2}}{\mu _z} + \frac{{{\sigma _z}^2}}{{{\sigma _x}^2 + {\sigma _z}^2}}{\mu _x}\\ K = \frac{{{\sigma _x}^2}}{{{\sigma _x}^2 + {\sigma _z}^2}}\\ \Rightarrow \mu = K{\mu _z} + \left( {1 - K} \right){\mu _x} = {\mu _x} + K\left( {{\mu _z} - {\mu _x}} \right)\\ \therefore {\sigma ^2} = \frac{{{\sigma _x}^2{\sigma _z}^2}}{{{\sigma _x}^2 + {\sigma _z}^2}} = K{\sigma _z}^2 = \left( {1 - K} \right){\sigma _x}^2 \end{array}\\$

代入变量得：

$\left\{ \begin{array}{l} K = PH^T/\left( {HPH^T + R} \right)\\ x = Hx + K\left( {z - Hx} \right)\\ P = (1 - KH)P \end{array} \right.\\$

上式即为Kalman滤波器是更新环节，其中H是从状态变量到观测量/输出变量的转换矩阵。

3.2实验分析

我们可以看到的是，Kalman滤波器有很多参数，除去运动模型形式假设的F和B参数，存在有多个协方差矩阵P、Q、R。下面我们逐一分析各个参数的影响。

协方差矩阵变化规律

在此之前可以看看不经过更新校正的状态变量均值和方差的变化，假设有如下的运动方式：

$\begin{array}{l} \left\{ \begin{array}{l} x = x + \Delta tv\\ v = v \end{array} \right. \Rightarrow \left[ {\begin{array}{*{20}{c}} x\\ v \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} 1&{\Delta t}\\ 0&1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} x\\ v \end{array}} \right]\\ \Rightarrow P = \left[ {\begin{array}{*{20}{c}} 1&{\Delta t}\\ 0&1 \end{array}} \right]P{\left[ {\begin{array}{*{20}{c}} 1&{\Delta t}\\ 0&1 \end{array}} \right]^T} \end{array}\\$

则有状态变量分布如下：

可以看到，在不引入量测的情况下，物体一直保持匀速直线运动，所以其误差的协方差分布一直向水平方向倾斜。

(1)不同Kalman模型

下面我们分别用一阶和二阶的Kalman滤波器去跟踪一个直线运动的物体，其中一阶Kalman滤波完全依赖量测的矫正，二阶Kalman滤波加入了速度因素，可见二阶模型跟踪效果更好，不过其实在这里，如果加入控制变量u，也能恰好达到匀速直线运动的效果。

(2) R和Q的影响

对于匀速直线运动，我们保持量测误差R不变，对比运动估计误差Q发现，Q越小，模型越相信运动规律，而模型正好也是匀速直线运动，因此跟踪效果更好。而当R变大时，模型会更加不相信量测结果，从而使得状态变量的协方差越来越大，但是由于预测环节模型的准确性，跟踪依然比较准确，可以从图中看出，当初始状态偏差很大时，模型不相信量测，导致跟踪轨迹很难与目标轨迹一致，而当R变小却可以重新跟踪到。

(3)P的影响

对于上面两幅图，表面上看上去P=1时，跟踪轨迹跟贴近于真实轨迹，但是如果将协方差矩阵P中的参数绘制出来即为：

我们可以发现，后者关于位置的方差变化趋势比较复杂，虽然二者均能跟踪到，但是当初始状态估计不好时，P过小会使得跟踪周期变长，而P较大时跟踪效果没有明显降低，因此通常P取值较大。

kalman filter java_Kalman filters（一）相关推荐

kalman filter java_Kalman Filter算法详解
关键词:卡尔曼滤波线性最优估计迭代预测值估计值协方差矩阵状态转移方程观测方程最小均方估计预测+矫正经典的卡尔曼滤波是一个迭代的线性的最优状态估计器.利用最小均方误差原理,可以保证状 ...
Error-State Kalman filter (ESKF)
1. 原理 1.1. 变量定义 1.2. 名义量预测 1.3. 误差量预测 1.4. 更新 1.5. 重置 2. 主要优点 The orientation error-state is minimal ...
一文图解卡尔曼滤波(Kalman Filter)
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达译者注:这恐怕是全网有关卡尔曼滤波最简单易懂的解释,如果你认真的读 ...
[Math]理解卡尔曼滤波器 (Understanding Kalman Filter)
2. 基本模型 2.1 系统模型卡尔曼滤波模型假设k时刻的真实状态是从(k − 1)时刻的状态演化而来,符合下式: (1) Fk 是作用在 Xk−1 上的状态变换模型(/矩阵/矢量). Bk 是作 ...
Control~Kalman filter
常用控制算法(包括PID和卡尔曼滤波等)各有什么天然的局限乃至缺陷回答都专业而又接地气自平衡小车的探讨:卡尔曼滤波与PID算法 ...
matlab温度数据怎么滤波_卡尔曼滤波算法思想理解 Kalman filter 第一篇
卡尔曼滤波算法思想理解 Kalman filter 第一篇最近在初步的理解目标跟踪的领域, 其中一个非常经典的算法卡尔曼滤波Kalman filter是需要有很好的理解才行, 由于已经脱离了学校,懂 ...
【UWB】Kalman filter, KF卡尔曼滤波, EKF 扩展卡尔曼滤波
文章目录卡尔曼滤波器扩展卡尔曼滤波器协方差 Ref: 卡尔曼滤波器首先从工程上看卡尔曼滤波算法. 引入一个离散控制过程的系统,该系统可用一个线性随机微分方程(linear stochastic ...
卡尔曼滤波器算法（Kalman Filter）—— 数学推导，图文并茂
(1)HMM:隐变量是离散的 (2)Kalman Filter:又叫 Linear Dynamic Model 或 Linear Gaussian Model 隐变量和观测变量都是连续的,都是服从高斯 ...
OpenCV‘s Kalman filter卡尔曼滤波器的实例(附完整代码)
OpenCV's Kalman filter卡尔曼滤波器的实例 OpenCV's Kalman filter卡尔曼滤波器的实例 OpenCV's Kalman filter卡尔曼滤波器的实例 #inc ...

kalman filter java_Kalman filters（一）

kalman filter java_Kalman filters（一）相关推荐

最新文章

热门文章