zoukankan
html css js c++ java
卡尔曼
#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;
}
查看全文
相关阅读:
在C++中,子类重载一个操作符时,如何调用父类被重载的操作符函数
基于AT89C51单片机的贪吃蛇电子游戏(仿真)
七种机器内部排序的原理与C语言实现,并计算它们的比较次数与移动次数。
使用java反射机制编写Student类并保存
SqlServer2012导入Oracle详细案例
TableLayoutPanel 的使用
windows 10 & Office 2016 安装
用过的手机集合
windows 7 语言切换 Vistalizator
ALV报表的颜色设计(行、列、单元格)
原文地址:https://www.cnblogs.com/wqj1212/p/1011432.html
最新文章
爬取下载图片,代码写得也是难看死,有时间优化吧
爬取拉钩上的招聘信息,写入到xls中
爬取豆瓣电影,把电影名称和详情url保存到json中
kubernetes入门实践
【docker 一】入门实践、环境部署、基本操作指令、镜像库、数据卷
读周志华《机器学习》前三章个人读书笔记
如何利用Chrome工具进行前端js调试
MySQL的一点浅显知识
在Eclipse如何实现在xml文件实现代码提示
Editplus配置java运行环境以及其他需求的简单设置
热门文章
学java网络编程的心得体会
Linux系统目录结构
grep 正则表达式的使用方法
Java多维数组使用注意事项
final关键字的注意事项
Java表达式的执行顺序
Linux用户管理
Linux档案属性与目录配置
C++的多态实现三要素
C++接口与实现的抽象分离
Copyright © 2011-2022 走看看