卡尔曼滤波算法

腾昵猫

已于 2023-02-16 11:23:17 修改

阅读量358

点赞数

分类专栏：一些算法文章标签：算法人工智能

于 2023-02-13 11:35:47 首次发布

本文链接：https://blog.csdn.net/Dr_Jack/article/details/129005622

版权

一些算法专栏收录该内容

3 篇文章 0 订阅

订阅专栏

基础原理

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

查看运行结果：