Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F85534062
ekf_node.cpp
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Sun, Sep 29, 18:22
Size
7 KB
Mime Type
text/x-c
Expires
Tue, Oct 1, 18:22 (2 d)
Engine
blob
Format
Raw Data
Handle
21193850
Attached To
R1517 test_package
ekf_node.cpp
View Options
#include "ekf_node.h"
using
namespace
casadi
;
void
KiteEKF_Node
::
filterCallback
(
const
geometry_msgs
::
PoseStamped
::
ConstPtr
&
msg
)
{
/** update measurement */
boost
::
unique_lock
<
boost
::
mutex
>
scoped_lock
(
m_mutex
);
measurements
.
push_back
(
*
msg
);
}
void
KiteEKF_Node
::
controlCallback
(
const
std_msgs
::
Int32MultiArray
::
ConstPtr
&
msg
)
{
double
throttle
=
static_cast
<
double
>
(
msg
->
data
[
0
]);
double
elevator
=
static_cast
<
double
>
(
msg
->
data
[
1
]);
double
rudder
=
static_cast
<
double
>
(
msg
->
data
[
2
]);
control
=
DM
::
vertcat
({
throttle
,
elevator
,
rudder
});
}
KiteEKF_Node
::
KiteEKF_Node
(
const
ros
::
NodeHandle
&
_nh
,
const
Function
&
Integrator
,
const
Function
&
Jacobian
)
{
/** @redo model initialization*/
nh
=
std
::
make_shared
<
ros
::
NodeHandle
>
(
_nh
);
measurements
.
set_capacity
(
2
);
kite_state
.
twist
.
resize
(
1
);
kite_state
.
transforms
.
resize
(
1
);
kite_state
.
joint_names
.
resize
(
1
);
kite_state
.
header
.
frame_id
=
"kite"
;
kite_state
.
joint_names
[
0
]
=
"lox"
;
brf_offset
=
DMVector
{
-
0.09
,
0
,
-
0.02
};
brf_rotation
=
DMVector
{
0.0
,
-
1.0
,
0.0
,
0.0
};
/** create filter object */
filter
=
std
::
make_shared
<
KiteEKF
>
(
Integrator
,
Jacobian
);
/** initialize subscribers and publishers */
state_pub
=
nh
->
advertise
<
sensor_msgs
::
MultiDOFJointState
>
(
"kite_state"
,
100
);
std
::
string
pose_topic
=
"/optitrack_client/RigidBody1/pose"
;
std
::
string
control_topic
=
"/chatter"
;
pose_sub
=
nh
->
subscribe
(
pose_topic
,
1000
,
&
KiteEKF_Node
::
filterCallback
,
this
);
control_sub
=
nh
->
subscribe
(
control_topic
,
1000
,
&
KiteEKF_Node
::
controlCallback
,
this
);
m_initialized
=
false
;
}
void
KiteEKF_Node
::
initialize
()
{
/** initialize kite state based on 2 sequential pose measurements */
if
(
measurements
.
size
()
==
2
)
{
DM
m_prev
=
optitrack2world
(
convertToDM
(
measurements
[
0
]));
DM
m_new
=
optitrack2world
(
convertToDM
(
measurements
[
1
]));
double
dt
=
measurements
[
1
].
header
.
stamp
.
toSec
()
-
measurements
[
0
].
header
.
stamp
.
toSec
();
/** linear velocity estimation */
DM
rdot
=
(
m_prev
(
Slice
(
0
,
3
),
0
)
-
m_new
(
Slice
(
0
,
3
),
0
)
)
/
dt
;
SX
att
=
m_prev
(
Slice
(
3
,
7
),
0
);
DM
att_inv
=
kite_utils
::
quat_inverse
(
att
);
SX
rdot_b
=
kite_utils
::
quat_multiply
(
att_inv
,
SX
::
vertcat
({
0
,
rdot
}));
DM
v_body
=
kite_utils
::
quat_multiply
(
rdot_b
,
att
);
v_body
=
v_body
(
Slice
(
1
,
4
),
0
);
/** estimate angular rates */
SX
att2
=
m_new
(
Slice
(
3
,
7
),
0
);
DM
dq
=
kite_utils
::
quat_multiply
(
SX
(
att_inv
),
att2
);
DM
identity
=
DM
::
vertcat
({
1
,
0
,
0
,
0
});
DM
qw
=
(
2.0
/
dt
)
*
(
dq
-
identity
);
DM
w_body
=
qw
(
Slice
(
1
,
4
),
0
);
std
::
cout
<<
"Initializaed at: "
<<
v_body
<<
" "
<<
w_body
<<
"
\n
"
;
std
::
vector
<
double
>
v_brf
=
v_body
.
nonzeros
();
std
::
vector
<
double
>
w_brf
=
w_body
.
nonzeros
();
std
::
vector
<
double
>
pose_irf
=
m_new
.
nonzeros
();
kite_state
.
twist
[
0
].
linear
.
x
=
v_brf
[
0
];
kite_state
.
twist
[
0
].
linear
.
y
=
v_brf
[
1
];
kite_state
.
twist
[
0
].
linear
.
z
=
v_brf
[
2
];
kite_state
.
twist
[
0
].
angular
.
x
=
w_brf
[
3
];
kite_state
.
twist
[
0
].
angular
.
y
=
w_brf
[
4
];
kite_state
.
twist
[
0
].
angular
.
z
=
w_brf
[
5
];
kite_state
.
transforms
[
0
].
translation
.
x
=
pose_irf
[
0
];
kite_state
.
transforms
[
0
].
translation
.
y
=
pose_irf
[
1
];
kite_state
.
transforms
[
0
].
translation
.
z
=
pose_irf
[
2
];
kite_state
.
transforms
[
0
].
rotation
.
w
=
pose_irf
[
3
];
kite_state
.
transforms
[
0
].
rotation
.
x
=
pose_irf
[
4
];
kite_state
.
transforms
[
0
].
rotation
.
y
=
pose_irf
[
5
];
kite_state
.
transforms
[
0
].
rotation
.
z
=
pose_irf
[
6
];
/** update filter time */
filter
->
setTime
(
measurements
[
1
].
header
.
stamp
.
toSec
()
);
// SET ESTIMATION !!!
//filter->setEstimation();
m_initialized
=
true
;
}
}
DM
KiteEKF_Node
::
convertToDM
(
const
geometry_msgs
::
PoseStamped
&
_value
)
{
DM
value
=
DM
::
zeros
(
7
);
value
[
0
]
=
_value
.
pose
.
position
.
x
;
value
[
1
]
=
_value
.
pose
.
position
.
y
;
value
[
2
]
=
_value
.
pose
.
position
.
z
;
value
[
3
]
=
_value
.
pose
.
orientation
.
w
;
value
[
4
]
=
_value
.
pose
.
orientation
.
x
;
value
[
5
]
=
_value
.
pose
.
orientation
.
y
;
value
[
6
]
=
_value
.
pose
.
orientation
.
z
;
return
value
;
}
DM
KiteEKF_Node
::
optitrack2world
(
const
DM
&
opt_pose
)
{
DM
position
=
opt_pose
(
Slice
(
0
,
3
),
0
);
DM
attitude
=
opt_pose
(
Slice
(
3
,
7
),
0
);
DM
tmp
=
kite_utils
::
quat_multiply
(
attitude
,
DM
::
vertcat
({
0
,
brf_offset
}));
DM
offset_b
=
kite_utils
::
quat_multiply
(
tmp
,
kite_utils
::
quat_inverse
(
attitude
));
position
+=
offset_b
(
Slice
(
1
,
4
),
0
);
/** transform to world ref frame */
tmp
=
kite_utils
::
quat_multiply
(
brf_rotation
,
DM
::
vertcat
({
0
,
position
}));
DM
world_pos
=
kite_utils
::
quat_multiply
(
tmp
,
kite_utils
::
quat_inverse
(
brf_rotation
));
world_pos
=
world_pos
(
Slice
(
1
,
4
),
0
);
tmp
=
kite_utils
::
quat_multiply
(
brf_rotation
,
attitude
);
DM
world_att
=
kite_utils
::
quat_multiply
(
tmp
,
kite_utils
::
quat_inverse
(
brf_rotation
));
return
DM
::
vertcat
({
world_pos
,
world_att
});
}
void
KiteEKF_Node
::
estimate
()
{
//get observation
DM
observation
=
optitrack2world
(
convertToDM
(
measurements
[
1
]));
double
tstamp
=
measurements
[
1
].
header
.
stamp
.
toSec
();
filter
->
estimate
(
observation
,
tstamp
);
/** update filter time */
filter
->
setTime
(
tstamp
);
}
void
KiteEKF_Node
::
publish
()
{
/** pack estimation to ROS message */
std
::
vector
<
double
>
state_vec
=
filter
->
getEstimation
().
nonzeros
();
kite_state
.
header
.
stamp
=
ros
::
Time
::
now
();
kite_state
.
twist
[
0
].
linear
.
x
=
state_vec
[
0
];
kite_state
.
twist
[
0
].
linear
.
y
=
state_vec
[
1
];
kite_state
.
twist
[
0
].
linear
.
z
=
state_vec
[
2
];
kite_state
.
twist
[
0
].
angular
.
x
=
state_vec
[
3
];
kite_state
.
twist
[
0
].
angular
.
y
=
state_vec
[
4
];
kite_state
.
twist
[
0
].
angular
.
z
=
state_vec
[
5
];
kite_state
.
transforms
[
0
].
translation
.
x
=
state_vec
[
6
];
kite_state
.
transforms
[
0
].
translation
.
y
=
state_vec
[
7
];
kite_state
.
transforms
[
0
].
translation
.
z
=
state_vec
[
8
];
kite_state
.
transforms
[
0
].
rotation
.
w
=
state_vec
[
9
];
kite_state
.
transforms
[
0
].
rotation
.
x
=
state_vec
[
10
];
kite_state
.
transforms
[
0
].
rotation
.
y
=
state_vec
[
11
];
kite_state
.
transforms
[
0
].
rotation
.
z
=
state_vec
[
12
];
/** publish current state estimation */
state_pub
.
publish
(
kite_state
);
}
int
main
(
int
argc
,
char
**
argv
)
{
ros
::
init
(
argc
,
argv
,
"ekf_node"
);
ros
::
NodeHandle
n
;
unsigned
tries
=
0
;
unsigned
locks
=
0
;
/** create a kite object */
std
::
string
kite_params_file
;
n
.
param
<
std
::
string
>
(
"kite_params"
,
kite_params_file
,
"umx_radian.yaml"
);
std
::
cout
<<
kite_params_file
<<
"
\n
"
;
KiteProperties
kite_props
=
kite_utils
::
LoadProperties
(
kite_params_file
);
AlgorithmProperties
algo_props
;
algo_props
.
Integrator
=
RK4
;
algo_props
.
sampling_time
=
0.02
;
KiteDynamics
kite
=
KiteDynamics
(
kite_props
,
algo_props
);
/** create a rigid body topic */
RigidBodyKinematics
rigid_body
=
RigidBodyKinematics
(
algo_props
);
Function
fIntegrator
=
rigid_body
.
getNumericIntegrator
();
Function
fJacobian
=
rigid_body
.
getNumericJacobian
();
/** create an EKF instance */
KiteEKF_Node
filter
(
n
,
fIntegrator
,
fJacobian
);
ros
::
Rate
loop_rate
(
50
);
/** 50 Hz */
while
(
ros
::
ok
())
{
//std::cout << "TRIES: " << tries << " LOCKS: " << locks << "\n";
/** Some complicated logic here please */
if
(
!
filter
.
initialized
())
{
std
::
cout
<<
"CANNOT INITIALIZE
\n
"
;
boost
::
unique_lock
<
boost
::
mutex
>
scoped_lock
(
filter
.
m_mutex
);
filter
.
initialize
();
}
else
{
/** perform estimation */
tries
++
;
boost
::
unique_lock
<
boost
::
mutex
>
scoped_lock
(
filter
.
m_mutex
);
locks
++
;
filter
.
estimate
();
filter
.
publish
();
}
ros
::
spinOnce
();
loop_rate
.
sleep
();
}
return
0
;
}
Event Timeline
Log In to Comment