卡尔曼滤波算法

目录

基础原理

详细方法

改进思路

参考文档

---

基础原理

卡尔曼滤波器是一种以模型为基础，融合概率和观测值最终估算出目标最大概率所在位置的算法。首先基础的卡尔曼滤波算法需要使用者提供目标运动的模型（H）、协方差矩阵（P）、观测值协方差（R），作为滤波算法运行的先验参数。更加复杂的卡尔曼滤波算法还需要提供控制向量表示目标的主动运动状态（u）以及控制矩阵表示目标的主动运动（B），主动运动没有描述的误差（Q）。卡尔曼滤波过程中会生成一个变量，称之为卡尔曼增益（K）。

详细方法

卡尔曼滤波使用计算链的方式可展示如下：

 

 首先模型会根据目标当前状态（X<t-1>、P<t-1>）计算出模型的预判值X'<t-1>、P'<t-1>，可以理解为目标运行后所在位置的概率分布（是一个方差为P'<t-1>，均值为X'<t-1>的高斯分布）。观测值融合步骤中会根据观测值分布（均值为Z，误差方差为R）结合模型输出的预判分布计算出联合分布并获取其均值位置（两个高斯分布联合概率还是高斯分布，均值和方差可计算）作为最新目标的位置。

用代码模拟这个过程：

#include <algorithm>
#include <vector>

#include "mat.hpp"
#include "base_function.hpp"
#include "weight_initilizer.hpp"



int main(int argc, char** argv)
{
	mat<6, 1, double> X({0., 0., 1., 2., .2, .1});		// * x,y,vx,vy,ax,ay
	printf("X<t-1>:");
	X.print();
	double t = 1.;
	// * 运行模型
	mat<6, 6, double> F({
		  1., 0., t , 0., .5 * t * t , 0.
		, 0., 1., 0., t , 0. 		 , .5 * t * t
		, 0., 0., 1., 0., t			 , 0.
		, 0., 0., 0., 1., 0.		 , t
		, 0., 0., 0., 0., 1.		 , 0.
		, 0., 0., 0., 0., 0.		 , 1.
	});
	// * 单位转化矩阵——假设单位一致
	mat<6, 6, double> H({
		  1., 0., 0 , 0., 0., 0.
		, 0., 1., 0., 0 , 0., 0.
		, 0., 0., 1., 0., 0., 0.
		, 0., 0., 0., 1., 0., 0.
		, 0., 0., 0., 0., 1., 0.
		, 0., 0., 0., 0., 0., 1.
	});
	// * 控制矩阵，设置成一个加速度随时间线性增长的变加速运动
	mat<6, 6, double> B({
		  0., 0., 0 , 0., 0., 0.
		, 0., 0., 0., 0 , 0., 0.
		, 0., 0., 0., 0., 0., 0.
		, 0., 0., 0., 0., 0., 0.
		, 0., 0., 0., 0., t, 0.
		, 0., 0., 0., 0., 0., t
	});
	// * 控制变量U
	mat<6, 1, double> U({
		  0.
		, 0.
		, 0.
		, 0.
		, 0.1
		, 0.2
	});
	// * 协方差矩阵
	mat<6, 6, double> P({
		.02, .04, 0., 0., 0., 0.
		, .04, .02, 0., 0., 0., 0.
		, 0., 0., .02, .04, 0., 0.
		, 0., 0., .04, .02, 0., 0.
		, 0., 0., 0., 0., .02, .04
		, 0., 0., 0., 0., .04, .02
	});
	// * 模型误差协方差矩阵
	mat<6, 6, double> Q({
		.01, .01, 0., 0., 0., 0.
		, .01, .01, 0., 0., 0., 0.
		, 0., 0., .01, .01, 0., 0.
		, 0., 0., .01, .01, 0., 0.
		, 0., 0., 0., 0., .01, .01
		, 0., 0., 0., 0., .01, .01
	});
	// * 测量值协方差矩阵
	mat<6, 6, double> R({
		.01, .04, 0., 0., 0., 0.
		, .04, .01, 0., 0., 0., 0.
		, 0., 0., .01, .04, 0., 0.
		, 0., 0., .04, .01, 0., 0.
		, 0., 0., 0., 0., .01, .04
		, 0., 0., 0., 0., .04, .01
	});

	// * 模拟一个走3点的目标
	for (int i = 0; i < 3; ++i)
	{
		// * 观测值Z，认为制造一点误差
		auto Z = F.dot(X) + mat<6, 1, double>({.1, .1, 0., 0., 0., 0.});
		printf("Z:");
		Z.print();
		// * 模型预判值输出
		auto X_ = F.dot(X) + B.dot(U);
		auto P_ = F.dot(P).dot(F.t()) + Q;
		printf("X_:"); // * 模型预判出的目标位置
		X_.print();

		// * 卡尔曼增益K
		auto K = P_.dot(H.t()).dot(
			inverse(
				H.dot(P_).dot(H.t()) + R));
		P = P_ - K.dot(H).dot(P_);
		X = X_ + K.dot(Z - X_);
		printf("X<t>:");
		X.print();
	}

	return 0;
}

查看运行结果：

 

可以发现更新后的Xt处于观测值Z和模型预判值X_之间。现在大家知道为什么我们管这个方法叫卡尔曼滤波器了吧？ 实际就是将目标的观测值和运行模型进行关联并滤波，使其在观测值的基础上更加贴近于我们锁设定的运行模型。

改进思路

这里的卡尔曼滤波器会用到控制向量和控制矩阵，这两个值怎么得出？我认为可以使用目标的观测位置和我们滤波的结果的差值作为误差，将其进行反向传播，不断更新B值或者U值。

参考文档

1、详解卡尔曼滤波原理_engineerlixl的博客-CSDN博客_卡尔曼滤波

2、Backprop KF: Learning Discriminative Deterministic State Estimators 反向传播卡尔曼滤波:学习判别确定性状态估计器_手持电烙铁的侠客的博客-CSDN博客_反向传播卡尔曼滤波