Kalman滤波器C语言实现-优快云博客

本文链接：https://blog.youkuaiyun.com/yongjiankuang/article/details/78995061

本文介绍了一种基于C语言的Kalman滤波器实现方法，详细展示了Kalman滤波器的初始化、更新及预测过程，并提供了关键的矩阵运算实现。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

kalman的文章之前也写过一篇，具体参考：kalman，本文主要是基于c的实现，若有不当之处，望指教，谢谢！

typedef struct image_double
{
	double* pix;
	unsigned int w;
	unsigned int h;
}*image_double_ptr;

#ifndef _KALMAN_H_
#define _KALMAN_H_

#include "img.h"
#include "img_utils.h"
#include "xmalloc.h"
#include "common.h"
#include "matrix.h"


typedef struct kalman
{
	//成员变量
	int state_size;
	int mea_size;
	image_double_ptr statePre;
	image_double_ptr statePost;
	image_double_ptr transitionMatrix;
	image_double_ptr measurementMatrix;
	image_double_ptr processNoiseCov;
	image_double_ptr measurementNoiseCov;
	image_double_ptr errorCovPre;
	image_double_ptr KK;
	image_double_ptr errorCovPost;
	

}kalman_t;

//成员函数
void kalman_init(kalman_t *k);
void kalman_update(kalman_t *k,image_double_ptr Y);
image_double_ptr kalman_predicted(kalman_t *k,image_double_ptr Y);
void free_kalman_param(kalman_t *k);

#endif;

#include "kalman.h"
#define rand10_10() (rand() % 21 - 10)

float randf()
{
	return (float)(rand() % 20) + (float)(rand() % 1001) / 1000 - 10;
}

//kalman模型参数初始化
void kalman_init(kalman_t *k)
{
	//分配内存
	int state_size = k->state_size; 
	int mea_size = k->mea_size;
	k -> statePre = new_image_double(1,state_size);
	k -> statePost = new_image_double(1,state_size);
	k -> transitionMatrix = new_image_double(state_size,state_size);
	k -> measurementMatrix = new_image_double(state_size,mea_size);
	k -> processNoiseCov = new_image_double(state_size,state_size);
	k -> measurementNoiseCov = new_image_double(mea_size,mea_size);
	k -> errorCovPre = new_image_double(state_size,state_size);
	k->KK = new_image_double(mea_size, state_size);
	k -> errorCovPost = new_image_double(state_size,state_size);

	//初始化
	set_identity(k -> measurementMatrix, 1);
	set_identity(k -> processNoiseCov,1e-5);
	set_identity(k -> measurementNoiseCov,1e-2);
	set_identity(k -> errorCovPost,1);

	for (int i = 0; i < state_size; i++)
		k->statePost->pix[i] = randf();
}

//kalman模型参数矫正
void kalman_update(kalman_t *k,image_double_ptr Y)
{
	image_double_ptr m1, m2, m3, m4;
	//KK;
	m1 = matrix_mul(k -> measurementMatrix,k -> errorCovPre);
	m2 = matrix_t(k->measurementMatrix);
	m3 = matrix_mul(m1, m2);
	free_image_double(m1);
	m1 = matrix_add(m3,k -> measurementNoiseCov);
	free_image_double(m3);
	m3 = matrix_diag_inv(m1);
	free_image_double(m1);
	m1 = matrix_mul(k->errorCovPre,m2);
	free_image_double(m2);
	k -> KK = matrix_mul(m1,m3);
	free_image_double(m1);
	free_image_double(m3);

	//statePost
	m1 = matrix_mul(k -> measurementMatrix,k -> statePre);
	m2 = matrix_sub(Y,m1);
	m3 = matrix_mul(k -> KK,m2);
	free_image_double(m1);
	free_image_double(m2);
	k->statePost = matrix_add(k -> statePre,m3);
	free_image_double(m3);

	//errorCovPost
	m1 = matrix_mul(k -> KK,k -> measurementMatrix);
	m2 = matrix_mul(m1, k->errorCovPre);
	k->errorCovPost = matrix_sub(k -> errorCovPre,m2);
	free_image_double(m1);
	free_image_double(m2);
}

//kalman模型预测
image_double_ptr kalman_predicted(kalman_t* k,image_double_ptr Y)
{
	image_double_ptr m1, m2, m3;
	
	//statePre
	k->statePre = matrix_mul(k -> transitionMatrix,k -> statePost);

	//errorCovPre
	m1 = matrix_t(k -> transitionMatrix);
	m2 = matrix_mul(k->transitionMatrix,k->errorCovPost);
	m3 = matrix_mul(m2,m1);
	k->errorCovPre = matrix_add(m3,k -> processNoiseCov);
	free_image_double(m1);
	free_image_double(m2);
	free_image_double(m3);

	//update
	kalman_update(k,Y);

	return k->statePost;
}

void free_kalman_param(kalman_t *k)
{
	if (k->statePre != NULL)
		free(k -> statePre);
	if (k->statePost != NULL)
		free(k -> statePost);
	if (k->transitionMatrix)
		free(k -> transitionMatrix);
	if (k->measurementMatrix != NULL)
		free(k -> measurementMatrix);
	if (k->processNoiseCov != NULL)
		free(k -> processNoiseCov);
	if (k->measurementNoiseCov != NULL)
		free(k -> measurementNoiseCov);
	if (k->errorCovPre != NULL)
		free(k -> errorCovPre);
	if (k->errorCovPost != NULL)
		free(k -> errorCovPost);
	if (k->KK != NULL)
		free(k -> KK);

	free(k);
}

由于kalman计算过程中涉及到矩阵的基本运算，下面是矩阵基本操作的实现：

#ifndef _MATRIX_H_
#define _MATRIX_H_

#include "img.h"
#include "img_utils.h"
#include "xmalloc.h"
#include "common.h"

//矩阵的转置
image_double_ptr matrix_t(image_double_ptr a);

//矩阵的乘法
image_double_ptr matrix_mul(image_double_ptr a, image_double_ptr b);

//矩阵加法
image_double_ptr matrix_add(image_double_ptr a, image_double_ptr b);

//矩阵减法
image_double_ptr matrix_sub(image_double_ptr a, image_double_ptr b);

//对角矩阵的逆
image_double_ptr matrix_diag_inv(image_double_ptr a);

//打印矩阵
void printMatrix(image_double_ptr a);

#endif;

#include "matrix.h"


//矩阵的转置
image_double_ptr matrix_t(image_double_ptr a)
{
	image_double_ptr c = new_image_double(a -> h,a -> w);

	for (int y = 0; y < a->h; y++)
	{
		for (int x = 0; x < a->w; x++)
		{
			c->pix[x * c -> w + y] = a->pix[y * a->w + x];
		}
	}

	return c;
}

//矩阵的乘法
image_double_ptr matrix_mul(image_double_ptr a, image_double_ptr b)
{
	int ar = a->h;
	int ac = a->w;
	int br = b->h;
	int bc = b->w;

	image_double_ptr c = new_image_double(bc,ar);

	for (int y = 0; y < ar; y++)
	{
		for (int x = 0; x < bc; x++)
		{
			for (int k = 0; k < ac; k++)
				c->pix[y * c -> w + x] += a->pix[y * a->w + k] * b->pix[k * b->w + x];
		}
	}

	return c;
}

//矩阵加法
image_double_ptr matrix_add(image_double_ptr a, image_double_ptr b)
{
	if (a->w != b->w || a->h != b->h)
		error("matrix_add: invalid input");
	image_double_ptr c = new_image_double(a -> w,a -> h);
	
	for (int y = 0; y < a->h; y++)
	{
		for (int x = 0; x < a->w; x++)
		{
			c->pix[y * a->w + x] = a->pix[y * a->w + x] + b->pix[y * b->w + x];
		}
	}

	return c; 
}

//矩阵减法
image_double_ptr matrix_sub(image_double_ptr a, image_double_ptr b)
{
	if (a->w != b->w || a->h != b->h)
		error("matrix_add: invalid input");
	image_double_ptr c = new_image_double(a->w, a->h);

	for (int y = 0; y < a->h; y++)
	{
		for (int x = 0; x < a->w; x++)
		{
			c->pix[y * a->w + x] = a->pix[y * a->w + x] - b->pix[y * b->w + x];
		}
	}

	return c;
}


//对角矩阵的逆
image_double_ptr matrix_diag_inv(image_double_ptr a)
{
	image_double_ptr c = new_image_double(a -> w,a -> h);

	for (int y = 0; y < a->h; y++)
	{
		for (int x = 0; x < a->w; x++)
		{
			if (y == x)
			{
				c->pix[y * a->w + x] = 1 / a->pix[y * a->w + x];
			}
		}
	}

	return c;
}

//打印矩阵
void printMatrix(image_double_ptr a)
{
	printf("-----------------------------------\n");
	for (int y = 0; y < a->h; y++)
	{
		for (int x = 0; x < a->w; x++)
		{
			printf(" %.2f ",a -> pix[y * a -> w + x]);
		}
		printf("\n");
	}

}

kalman之c语言实现