c语言卡尔曼函数库,kalman之c语言实现

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");

}

}