卡尔曼滤波简介＋ 算法实现代码(C/C++)(转)

原帖地址：/article/4931121.html

最佳线性滤波理论起源于
40

年代美国科学家
Wiener

和前苏联科学家Ｋ
олмогоров

等人的研究工作，后人统称为维纳滤波理论。从理论上说，维纳滤波的最大缺点是必须用到无限过去的数据，不适用于实时处理。为了克服这一缺点，
60

年代
Kalman

把
状态空间模型引入滤波理论，并导出了一套递推估计算法，后人称之为卡尔曼滤波理论。卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算
法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理
和计算机运算。

现设线性时变系统的离散状态防城和观测方程为：

X(k) = F(k,k-1)·X(k-1)+T(k,k-1)·U(k-1)

Y(k) = H(k)·X(k)+N(k)

其中

X(k)和Y(k)分别是k时刻的状态矢量和观测矢量

F(k,k-1)为状态转移矩阵

U(k)为k时刻动态噪声

T(k,k-1)为系统控制矩阵

H(k)为k时刻观测矩阵

N(k)为k时刻观测噪声

则卡尔曼滤波的算法流程为：

预估计X(k)^= F(k,k-1)·X(k-1)

计算预估计协方差矩阵

C(k)^=F(k,k-1)×C(k)×F(k,k-1)'+T(k,k-1)×Q(k)×T(k,k-1)'

Q(k) = U(k)×U(k)'

计算卡尔曼增益矩阵

K(k) = C(k)^×H(k)'×[H(k)×C(k)^×H(k)'+R(k)]^(-1)

R(k) = N(k)×N(k)'

更新估计

X(k)~=X(k)^+K(k)×[Y(k)-H(k)×X(k)^]

计算更新后估计协防差矩阵

C(k)~ = [I-K(k)×H(k)]×C(k)^×[I-K(k)×H(k)]'+K(k)×R(k)×K(k)'

X(k+1) = X(k)~

C(k+1) = C(k)~

重复以上步骤

其c语言实现代码如下：



#include
"
stdlib.h
"



#include
"
rinv.c
"



int
lman(n,m,k,f,q,r,h,y,x,p,g)



int
n,m,k;



double
f[],q[],r[],h[],y[],x[],p[],g[];







{
int
i,j,kk,ii,l,jj,js;



double

*
e,
*
a,
*
b;



e
=
malloc(m
*
m
*
sizeof
(
double
));



l
=
m;



if
(l
<
n) l
=
n;



a
=
malloc(l
*
l
*
sizeof
(
double
));



b
=
malloc(l
*
l
*
sizeof
(
double
));



for
(i
=
0
; i
<=
n
-
1
; i
++
)



for
(j
=
0
; j
<=
n
-
1
; j
++
)







{ ii
=
i
*
l
+
j; a[ii]
=
0.0
;



for
(kk
=
0
; kk
<=
n
-
1
; kk
++
)



a[ii]
=
a[ii]
+
p[i
*
n
+
kk]
*
f[j
*
n
+
kk];



}



for
(i
=
0
; i
<=
n
-
1
; i
++
)



for
(j
=
0
; j
<=
n
-
1
; j
++
)







{ ii
=
i
*
n
+
j; p[ii]
=
q[ii];



for
(kk
=
0
; kk
<=
n
-
1
; kk
++
)



p[ii]
=
p[ii]
+
f[i
*
n
+
kk]
*
a[kk
*
l
+
j];



}



for
(ii
=
2
; ii
<=
k; ii
++
)







{
for
(i
=
0
; i
<=
n
-
1
; i
++
)



for
(j
=
0
; j
<=
m
-
1
; j
++
)







{ jj
=
i
*
l
+
j; a[jj]
=
0.0
;



for
(kk
=
0
; kk
<=
n
-
1
; kk
++
)



a[jj]
=
a[jj]
+
p[i
*
n
+
kk]
*
h[j
*
n
+
kk];



}



for
(i
=
0
; i
<=
m
-
1
; i
++
)



for
(j
=
0
; j
<=
m
-
1
; j
++
)







{ jj
=
i
*
m
+
j; e[jj]
=
r[jj];



for
(kk
=
0
; kk
<=
n
-
1
; kk
++
)



e[jj]
=
e[jj]
+
h[i
*
n
+
kk]
*
a[kk
*
l
+
j];



}



js
=
rinv(e,m);



if
(js
==
0
)







{ free(e); free(a); free(b);
return
(js);}



for
(i
=
0
; i
<=
n
-
1
; i
++
)



for
(j
=
0
; j
<=
m
-
1
; j
++
)







{ jj
=
i
*
m
+
j; g[jj]
=
0.0
;



for
(kk
=
0
; kk
<=
m
-
1
; kk
++
)



g[jj]
=
g[jj]
+
a[i
*
l
+
kk]
*
e[j
*
m
+
kk];



}



for
(i
=
0
; i
<=
n
-
1
; i
++
)







{ jj
=
(ii
-
1
)
*
n
+
i; x[jj]
=
0.0
;



for
(j
=
0
; j
<=
n
-
1
; j
++
)



x[jj]
=
x[jj]
+
f[i
*
n
+
j]
*
x[(ii
-
2
)
*
n
+
j];



}



for
(i
=
0
; i
<=
m
-
1
; i
++
)







{ jj
=
i
*
l; b[jj]
=
y[(ii
-
1
)
*
m
+
i];



for
(j
=
0
; j
<=
n
-
1
; j
++
)



b[jj]
=
b[jj]
-
h[i
*
n
+
j]
*
x[(ii
-
1
)
*
n
+
j];



}



for
(i
=
0
; i
<=
n
-
1
; i
++
)







{ jj
=
(ii
-
1
)
*
n
+
i;



for
(j
=
0
; j
<=
m
-
1
; j
++
)



x[jj]
=
x[jj]
+
g[i
*
m
+
j]
*
b[j
*
l];



}



if
(ii
<
k)







{
for
(i
=
0
; i
<=
n
-
1
; i
++
)



for
(j
=
0
; j
<=
n
-
1
; j
++
)







{ jj
=
i
*
l
+
j; a[jj]
=
0.0
;



for
(kk
=
0
; kk
<=
m
-
1
; kk
++
)



a[jj]
=
a[jj]
-
g[i
*
m
+
kk]
*
h[kk
*
n
+
j];



if
(i
==
j) a[jj]
=
1.0
+
a[jj];



}



for
(i
=
0
; i
<=
n
-
1
; i
++
)



for
(j
=
0
; j
<=
n
-
1
; j
++
)







{ jj
=
i
*
l
+
j; b[jj]
=
0.0
;



for
(kk
=
0
; kk
<=
n
-
1
; kk
++
)



b[jj]
=
b[jj]
+
a[i
*
l
+
kk]
*
p[kk
*
n
+
j];



}



for
(i
=
0
; i
<=
n
-
1
; i
++
)



for
(j
=
0
; j
<=
n
-
1
; j
++
)







{ jj
=
i
*
l
+
j; a[jj]
=
0.0
;



for
(kk
=
0
; kk
<=
n
-
1
; kk
++
)



a[jj]
=
a[jj]
+
b[i
*
l
+
kk]
*
f[j
*
n
+
kk];



}



for
(i
=
0
; i
<=
n
-
1
; i
++
)



for
(j
=
0
; j
<=
n
-
1
; j
++
)







{ jj
=
i
*
n
+
j; p[jj]
=
q[jj];



for
(kk
=
0
; kk
<=
n
-
1
; kk
++
)



p[jj]
=
p[jj]
+
f[i
*
n
+
kk]
*
a[j
*
l
+
kk];



}



}



}



free(e); free(a); free(b);



return
(js);



}





C++实现代码如下：

============================
kalman.h
================================

//
kalman.h: interface for the kalman class.

//

/////////////////////////////////////////////////////////////////////
/

#if
!defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_)

#define
AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_

#if
_MSC_VER > 1000

#pragma once

#endif

//
_MSC_VER > 1000

#include
<
math.h
>

#include
"
cv.h
"

class
kalman

{

public
:

void
init_kalman(
int
x,
int
xv,
int
y,
int
yv);

CvKalman
*
cvkalman;

CvMat
*
state;

CvMat
*
process_noise;

CvMat
*
measurement;

const
CvMat
*
prediction;

CvPoint2D32f get_predict(
float
x,
float
y);

kalman(
int
x
=
0
,
int
xv
=
0
,
int
y
=
0
,
int
yv
=
0
);

//
virtual ~kalman();

};

#endif

//
!defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLUDED_)

============================
kalman.cpp
================================

#include
"
kalman.h
"

#include
<
stdio.h
>

/*
tester de printer toutes les valeurs des vecteurs


*/

/*
tester de changer les matrices du noises
*/

/*
replace state by cvkalman->state_post ???
*/

CvRandState rng;

const

double
T
=

0.1
;

kalman::kalman(
int
x,
int
xv,
int
y,
int
yv)

{

cvkalman
=
cvCreateKalman(
4
,
4
,
0
);

state
=
cvCreateMat(
4
,
1
, CV_32FC1 );

process_noise
=
cvCreateMat(
4
,
1
, CV_32FC1 );

measurement
=
cvCreateMat(
4
,
1
, CV_32FC1 );

int
code
=

-
1
;

/*
create matrix data
*/

const

float
A[]
=
{

1
, T,
0
,
0
,

0
,
1
,
0
,
0
,

0
,
0
,
1
, T,

0
,
0
,
0
,
1

};

const

float
H[]
=
{

1
,
0
,
0
,
0
,

0
,
0
,
0
,
0
,

0
,
0
,
1
,
0
,

0
,
0
,
0
,
0

};

const

float
P[]
=
{

pow(
320
,
2
), pow(
320
,
2
)
/
T,
0
,
0
,

pow(
320
,
2
)
/
T, pow(
320
,
2
)
/
pow(T,
2
),
0
,
0
,

0
,
0
, pow(
240
,
2
), pow(
240
,
2
)
/
T,

0
,
0
, pow(
240
,
2
)
/
T, pow(
240
,
2
)
/
pow(T,
2
)

};

const

float
Q[]
=
{

pow(T,
3
)
/
3
, pow(T,
2
)
/
2
,
0
,
0
,

pow(T,
2
)
/
2
, T,
0
,
0
,

0
,
0
, pow(T,
3
)
/
3
, pow(T,
2
)
/
2
,

0
,
0
, pow(T,
2
)
/
2
, T

};

const

float
R[]
=
{

1
,
0
,
0
,
0
,

0
,
0
,
0
,
0
,

0
,
0
,
1
,
0
,

0
,
0
,
0
,
0

};

cvRandInit(
&
rng,
0
,
1
,
-
1
, CV_RAND_UNI );

cvZero( measurement );

cvRandSetRange(
&
rng,
0
,
0.1
,
0
);

rng.disttype
=
CV_RAND_NORMAL;

cvRand(
&
rng, state );

memcpy( cvkalman
->
transition_matrix
->
data.fl, A,
sizeof
(A));

memcpy( cvkalman
->
measurement_matrix
->
data.fl, H,
sizeof
(H));

memcpy( cvkalman
->
process_noise_cov
->
data.fl, Q,
sizeof
(Q));

memcpy( cvkalman
->
error_cov_post
->
data.fl, P,
sizeof
(P));

memcpy( cvkalman
->
measurement_noise_cov
->
data.fl, R,
sizeof
(R));

//
cvSetIdentity( cvkalman->process_noise_cov, cvRealScalar(1e-5) );

//
cvSetIdentity( cvkalman->error_cov_post, cvRealScalar(1));

//
cvSetIdentity( cvkalman->measurement_noise_cov, cvRealScalar(1e-1) );

/*
choose initial state
*/

state
->
data.fl[
0
]
=
x;

state
->
data.fl[
1
]
=
xv;

state
->
data.fl[
2
]
=
y;

state
->
data.fl[
3
]
=
yv;

cvkalman
->
state_post
->
data.fl[
0
]
=
x;

cvkalman
->
state_post
->
data.fl[
1
]
=
xv;

cvkalman
->
state_post
->
data.fl[
2
]
=
y;

cvkalman
->
state_post
->
data.fl[
3
]
=
yv;

cvRandSetRange(
&
rng,
0
, sqrt(cvkalman
->
process_noise_cov
->
data.fl[
0
]),
0
);

cvRand(
&
rng, process_noise );

}

CvPoint2D32f kalman::get_predict(
float
x,
float
y){

/*
update state with current position
*/

state
->
data.fl[
0
]
=
x;

state
->
data.fl[
2
]
=
y;

/*
predict point position
*/

/*
x'k=A鈥k+B鈥k

P'k=A鈥k-1*AT + Q
*/

cvRandSetRange(
&
rng,
0
, sqrt(cvkalman
->
measurement_noise_cov
->
data.fl[
0
]),
0
);

cvRand(
&
rng, measurement );

/*
xk=A?xk-1+B?uk+wk
*/

cvMatMulAdd( cvkalman
->
transition_matrix, state, process_noise, cvkalman
->
state_post );

/*
zk=H?xk+vk
*/

cvMatMulAdd( cvkalman
->
measurement_matrix, cvkalman
->
state_post, measurement, measurement );

/*
adjust Kalman filter state
*/

/*
Kk=P'k鈥T鈥?H鈥'k鈥T+R)-1

xk=x'k+Kk鈥?zk-H鈥'k)

Pk=(I-Kk鈥)鈥'k
*/

cvKalmanCorrect( cvkalman, measurement );

float
measured_value_x
=
measurement
->
data.fl[
0
];

float
measured_value_y
=
measurement
->
data.fl[
2
];

const
CvMat
*
prediction
=
cvKalmanPredict( cvkalman,
0
);

float
predict_value_x
=
prediction
->
data.fl[
0
];

float
predict_value_y
=
prediction
->
data.fl[
2
];

return
(cvPoint2D32f(predict_value_x,predict_value_y));

}

void
kalman::init_kalman(
int
x,
int
xv,
int
y,
int
yv)

{

state
->
data.fl[
0
]
=
x;

state
->
data.fl[
1
]
=
xv;

state
->
data.fl[
2
]
=
y;

state
->
data.fl[
3
]
=
yv;

cvkalman
->
state_post
->
data.fl[
0
]
=
x;

cvkalman
->
state_post
->
data.fl[
1
]
=
xv;

cvkalman
->
state_post
->
data.fl[
2
]
=
y;

cvkalman
->
state_post
->
data.fl[
3
]
=
yv;

}