卡尔曼滤波算法

目录

基础原理

详细方法

改进思路

参考文档


基础原理

卡尔曼滤波器是一种以模型为基础,融合概率和观测值最终估算出目标最大概率所在位置的算法。首先基础的卡尔曼滤波算法需要使用者提供目标运动的模型(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博客_反向传播卡尔曼滤波

  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

腾昵猫

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值