void SegmentStitching::runNode(){
std::vector<std::vector<Line> > segmentLines;
mapping_msgs::SegmentObjectVector allSegmentObjects;
// Go through all the segments, extract measurements, and convert these to lines.
for (size_t segment = 0; segment < mapSegments.size(); segment++) {
ROS_INFO("Processing segment %d of %d", (int)(segment + 1), (int)(mapSegments.size()));
// first, get the set of points which represent the measurements taken
// in the segment.
pcl::PointCloud<pcl::PointXYZ>::Ptr measurements(new pcl::PointCloud<pcl::PointXYZ>);
// the objects vector contains the relative positions and strings
// corresponding to the objects which were detected in the segment. Only
// one message is received when the object is first detected -
// subsequent detections do not add more information, so there is no
// need to do clustering to find the object position, though this might
// increase the accuracy.
mapping_msgs::ObjectVector objects;
segmentToMeasurements(mapSegments[segment], measurements, objects);
// Extract the lines from this segment using ransac. This operation
// destroys the measurement pointcloud
std::vector<Line> lines = extractLinesFromMeasurements(measurements, ransacThreshold);
segmentLines.push_back(lines);
allSegmentObjects.segmentObjects.push_back(objects);
// tmpPublish(measurements, lines);
}
std::vector<std::vector<Line> > stitchedLines = processSegments(segmentLines, allSegmentObjects);
publishFinalMessages(stitchedLines, allSegmentObjects);
// publishSegmentLines(stitchedLines);
}
/**
* @brief estimated_pose Returns estimated object pose in cartesian frame
*/
geometry_msgs::Pose kalman_pose_estimation(geometry_msgs::Pose previous_pose_, geometry_msgs::Pose measured_pose_) {
// TODO: create kalman filter for each estimated object separatelly.
cv::KalmanFilter KF; // instantiate Kalman Filter
int nStates = 18; // the number of states
int nMeasurements = 6; // the number of measured states
int nInputs = 0; // the number of action control
double dt = 0.125; // time between measurements (1/FPS)
initKalmanFilter(KF, nStates, nMeasurements, nInputs, dt); // init function
cv::Mat measurements(nMeasurements, 1, CV_64F);
measurements.setTo(cv::Scalar(0));
// Get object pose in sensor frame.
tf::Transform measured_tf;
tf::poseMsgToTF (measured_pose_ , measured_tf);
// tf::btMatrix3x3 measured_rot = measured_tf.getBasis();
// Get the measured translation
cv::Mat translation_measured(3, 1, CV_64F);
// translation_measured = ... TODO!
// Get the measured rotation
cv::Mat rotation_measured(3, 3, CV_64F);
//rotation_measured = ... TODO!
// fill the measurements vector
fillMeasurements(measurements, translation_measured, rotation_measured);
// Instantiate estimated translation and rotation
cv::Mat translation_estimated(3, 1, CV_64F);
cv::Mat rotation_estimated(3, 3, CV_64F);
// update the Kalman filter with good measurements
updateKalmanFilter( KF, measurements, translation_estimated, rotation_estimated);
geometry_msgs::Pose estimated_pose;
return estimated_pose;
}
bool ParseFoodItem(std::ifstream & infile, FoodItem & item)
{
if (true == infile.eof())
{
return false;
}
const int MAX_TOKEN = 256;
std::string line;
std::string tokenBuf(MAX_TOKEN, '\0');
const char * delim = ",\n";
do
{
float calories = 0;
// read line
std::getline(infile, line);
// grab first token
char * token = nullptr;
char * nextToken = nullptr;
token = strtok_s(&line[0], delim, &nextToken);
std::string tokenStr(token);
//trim_str(tokenStr);
String nameToken(ConvertStringToWString(token)); // read name, 1st token
// next token plz
token = strtok_s(nullptr, delim, &nextToken);
if (token == nullptr)
{
return false;
}
// read calories, 2nd token
calories = (float)atof(token);
if (calories <= 0)
{
return false;
}
// read description
std::string desc;
std::getline(infile, desc);
String measurements(ConvertStringToWString(desc));
item = FoodItem(nameToken, measurements, calories);
return true;
}
while (false == infile.eof());
return false;
}
void measure_rand(int size_input, mpz_t *vec, int prime_size, mpz_t prime) {
vector<double> measurements(NUM_SAMPLES, 0);
char scratch_str[BUFLEN];
for (int i=0; i<NUM_SAMPLES; i++)
measurements[i] = do_rand_gen(size_input, vec, prime)/size_input;
snprintf(scratch_str, BUFLEN-1, "c_%d", prime_size);
print_stats(scratch_str, measurements);
}
cv::Mat MoveParameters::toCvMat() const {
cv::Mat measurements(6, 1, CV_64F);
measurements.at<double>(0) = x;
measurements.at<double>(1) = y;
measurements.at<double>(2) = z;
measurements.at<double>(3) = roll;
measurements.at<double>(4) = pitch;
measurements.at<double>(5) = yaw;
return measurements;
}
void measure_decrypt(int size_input, mpz_t *plain,
mpz_t *cipher, int prime_size, mpz_t prime) {
vector<double> measurements(NUM_SAMPLES, 0);
char scratch_str[BUFLEN];
for (int i=0; i<NUM_SAMPLES; i++) {
do_encrypt(size_input, plain, cipher, prime_size, prime);
measurements[i] = do_decrypt(size_input, plain, cipher, prime_size, prime)/size_input;
}
snprintf(scratch_str, BUFLEN-1, "d_%d", prime_size);
print_stats(scratch_str, measurements);
}
void measure_mult(int size_input, mpz_t *vec1, mpz_t *vec2,
mpz_t *vec3, int operand_size1, int operand_size2) {
vector<double> measurements(NUM_SAMPLES, 0);
char scratch_str[BUFLEN];
for (int i=0; i<NUM_SAMPLES; i++)
measurements[i] =
do_mult(size_input, vec1, vec2, vec3, operand_size1, operand_size2)/size_input;
snprintf(scratch_str, BUFLEN-1, "f_%d_%d", operand_size1, operand_size2);
print_stats(scratch_str, measurements);
}
void measure_field_mult(int size_input, mpz_t *vec1, mpz_t *vec2,
mpz_t *vec3, int prime_size, mpz_t prime) {
vector<double> measurements(NUM_SAMPLES, 0);
char scratch_str[BUFLEN];
for (int i=0; i<NUM_SAMPLES; i++)
measurements[i] =
do_field_mult(size_input, vec1, vec2, vec3, prime)/size_input;
snprintf(scratch_str, BUFLEN-1, "f_mod_%d", prime_size);
print_stats(scratch_str, measurements);
}
void DoorHandleDetectionNode::mainComputation(const sensor_msgs::PointCloud2::ConstPtr &image)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_bbox(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_sandwich(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_debug(new pcl::PointCloud<pcl::PointXYZ>);
vpColVector direction_line(3);
vpColVector normal(3);
vpHomogeneousMatrix dMp;
geometry_msgs::PoseStamped cMdh_msg;
vpColVector coeffs;
struct inliersAndCoefficients plane_variables;
pcl::PointCloud<pcl::PointXYZ>::Ptr plane(new pcl::PointCloud<pcl::PointXYZ>);
std_msgs::Int8 status_handle_msg;
//Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*image, *cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
//Downsample the point cloud given by the camera
pcl::VoxelGrid<pcl::PointXYZ> vg;
vg.setInputCloud( cloud );
vg.setLeafSize( 0.005f, 0.005f, 0.005f );
vg.filter(*cloud_filtered);
if(m_cam_is_initialized)
{
//Find the coefficients of the biggest plane of the point cloud
plane_variables = DoorHandleDetectionNode::getPlaneInliersAndCoefficients(cloud_filtered);
coeffs.stack(plane_variables.coefficients->values[0]);
coeffs.stack(plane_variables.coefficients->values[1]);
coeffs.stack(plane_variables.coefficients->values[2]);
coeffs.stack(plane_variables.coefficients->values[3]);
//Invert the normal in order to have the z axis toward the camera
normal[0] = -coeffs[0];
normal[1] = -coeffs[1];
normal[2] = -coeffs[2];
//Check if the size of the plane if big enough
if (plane_variables.inliers->indices.size() < 100)
{
std::cout << "Could not find a plane in the scene." << std::endl;
vpDisplay::displayText(m_img_, 60, 5, "No plane detected", vpColor::red);
vpDisplay::flush(m_img_);
}
else
{
if (debug)
{
//Copy the inliers of the plane to a new cloud.
plane = DoorHandleDetectionNode::createPlanePC(cloud, plane_variables.inliers, plane_variables.coefficients);
//Create the center of the plane
vpColVector centroidPlane(3);
centroidPlane = DoorHandleDetectionNode::getCentroidPCL(plane);
//Create a tf for the plane
dMp = DoorHandleDetectionNode::createTFPlane(coeffs, centroidPlane[0], centroidPlane[1], centroidPlane[2]);
}
//Compute the Z of the corners of the smaller detection
m_Z_topleft = -(coeffs[3] + m_height_dh)/(coeffs[0]*m_x_min + coeffs[1]*m_y_min + coeffs[2]) ;
m_Z_topright = -(coeffs[3] + m_height_dh)/(coeffs[0]*m_x_max + coeffs[1]*m_y_min + coeffs[2]) ;
m_Z_bottomright = -(coeffs[3] + m_height_dh)/(coeffs[0]*m_x_max + coeffs[1]*m_y_max + coeffs[2]) ;
m_Z_bottomleft = -(coeffs[3] + m_height_dh)/(coeffs[0]*m_x_min + coeffs[1]*m_y_max + coeffs[2]) ;
//Creating a cloud with all the points of the door handle
if (m_tracking_works)
{
cloud_bbox = DoorHandleDetectionNode::getOnlyUsefulHandle(cloud);
cloud_sandwich = DoorHandleDetectionNode::createPCLSandwich(cloud_bbox, coeffs);
}
else
cloud_sandwich = DoorHandleDetectionNode::createPCLSandwich(cloud, coeffs);
if (debug)
{
//Create a point cloud of only the top left and bottom right points used for the narrow detection
cloud_debug->width = 2;
cloud_debug->height = 1;
cloud_debug->points.resize (cloud_debug->width * cloud_debug->height);
cloud_debug->points[0].x = m_X_min;
cloud_debug->points[0].y = m_Y_min;
cloud_debug->points[0].z = m_Z_topleft;
cloud_debug->points[1].x = m_X_max;
cloud_debug->points[1].y = m_Y_max;
cloud_debug->points[1].z = m_Z_bottomright;
cloud_debug->header.frame_id = m_parent_depth_tf;
debug_pcl_pub.publish(cloud_debug);
//Publish the point cloud of the door handle with noise
cloud_sandwich->header.frame_id = m_parent_depth_tf;
pcl_dh_pub.publish(*cloud_sandwich);
//Publish the point clouds of the plane
plane->header.frame_id = m_parent_depth_tf;
pcl_plane_pub.publish(*plane);
}
//Check if the size of the point cloud of the door handle if big enough to localize it or not and check if the detection should be done or not
if (cloud_sandwich->size()<50 || m_stop_detection){
m_is_door_handle_present = 0;
ROS_INFO_STREAM("No door handle detected : " << cloud_sandwich->size());
m_tracking_works = false;
vpPixelMeterConversion::convertPoint(m_cam_depth, 0, 0, m_x_min, m_y_min);
vpPixelMeterConversion::convertPoint(m_cam_depth, m_img_mono.getWidth() - 1, m_img_mono.getHeight() - 1, m_x_max, m_y_max);
}
else
{
m_is_door_handle_present = 1;
//Get the axis from the cloud sandwich with the odr method
direction_line = getCoeffLineWithODR(cloud_sandwich);
//Set the axis of the door handle towards the right way
if (m_dh_right)
{
if (direction_line[0]<0)
{
direction_line[0] = -direction_line[0];
direction_line[1] = -direction_line[1];
direction_line[2] = -direction_line[2];
}
}
else
{
if (direction_line[0]>0)
{
direction_line[0] = -direction_line[0];
direction_line[1] = -direction_line[1];
direction_line[2] = -direction_line[2];
}
}
//Create the black&white image used to reduce the detection
DoorHandleDetectionNode::morphoSandwich(cloud_sandwich );
//Get the centroid of the door handle
vpColVector centroidDH = DoorHandleDetectionNode::getCentroidPCL(cloud_sandwich);
//Compute the pose of the door handle with respect to the Depth camera and publish a tf of it
m_dMh = DoorHandleDetectionNode::createTFLine(direction_line, normal, centroidDH[0], centroidDH[1], centroidDH[2]);
vpHomogeneousMatrix cMd;
for(unsigned int i=0; i<3; i++)
cMd[i][3] = m_extrinsicParam[i];
//Compute the pose of the door handle with respect to the RGB camera
m_cMh = cMd * m_dMh;
//Kalman Filter
vpTranslationVector T_cMh = m_cMh.getTranslationVector();
vpRotationMatrix R_cMh = m_cMh.getRotationMatrix();
cv::Mat translation_measured(3, 1, CV_64F);
cv::Mat rotation_measured(3, 3, CV_64F);
for(int i = 0; i < 3; i++){
for(int j = 0; j < 3; j++){
rotation_measured.at<double>(i,j) = R_cMh[i][j];
}
translation_measured.at<double>(i) = T_cMh[i];
}
cv::Mat measurements(6, 1, CV_64F);
// fill the measurements vector
fillMeasurements(measurements, translation_measured, rotation_measured);
// Instantiate estimated translation and rotation
cv::Mat translation_estimated(3, 1, CV_64F);
cv::Mat rotation_estimated(3, 3, CV_64F);
// update the Kalman filter with good measurements
updateKalmanFilter( m_KF, measurements, translation_estimated, rotation_estimated);
for(int i = 0; i < 3; i++){
for(int j = 0; j < 3; j++){
m_cMh_filtered_kalman[i][j] = rotation_estimated.at<double>(i,j) ;
}
m_cMh_filtered_kalman[i][3] = translation_estimated.at<double>(i);
}
//Publish the pose of the handle with respect to the camera
cMdh_msg.header.stamp = ros::Time::now();
cMdh_msg.pose = visp_bridge::toGeometryMsgsPose(m_cMh_filtered_kalman);
cMdh_msg.header.frame_id = m_parent_rgb_tf;
pose_handle_pub.publish(cMdh_msg);
}
}
}
//Publish the status of the door handle : 0 if no door handle, 1 if a door handle is found
status_handle_msg.data = m_is_door_handle_present;
door_handle_status_pub.publish( status_handle_msg );
}
USING_NAMESPACE_ACADO
int main( )
{
// DIFFERENTIAL STATES :
// -------------------------
DifferentialState r; // the length r of the cable
DifferentialState phi; // the angle phi
DifferentialState theta; // the angle theta (grosser wert- naeher am boden/kleiner wert - weiter oben)
// ------------------------- // -------------------------------------------
DifferentialState dr; // first derivative of r0 with respect to t
DifferentialState dphi; // first derivative of phi with respect to t
DifferentialState dtheta; // first derivative of theta with respect to t
// ------------------------- // -------------------------------------------
DifferentialState n; // winding number(rauslassen)
// ------------------------- // -------------------------------------------
DifferentialState Psi; // the roll angle Psi
DifferentialState CL; // the aerodynamic lift coefficient
// ------------------------- // -------------------------------------------
DifferentialState W; // integral over the power at the generator
// ( = ENERGY )(rauslassen)
// MEASUREMENT FUNCTION :
// -------------------------
Function model_response ; // the measurement function
// CONTROL :
// -------------------------
Control ddr0; // second derivative of r0 with respect to t
Control dPsi; // first derivative of Psi with respect to t
Control dCL; // first derivative of CL with respect to t
Disturbance w_extra;
// PARAMETERS :
// ------------------------
// PARAMETERS OF THE KITE :
// -----------------------------
double mk = 2000.00; // mass of the kite // [ kg ](maybe change to 5000kg)
double A = 500.00; // effective area // [ m^2 ]
double V = 720.00; // volume // [ m^3 ]
double cd0 = 0.04; // aerodynamic drag coefficient // [ ]
// ( cd0: without downwash )
double K = 0.04; // induced drag constant // [ ]
// PHYSICAL CONSTANTS :
// -----------------------------
double g = 9.81; // gravitational constant // [ m /s^2]
double rho = 1.23; // density of the air // [ kg/m^3]
// PARAMETERS OF THE CABLE :
// -----------------------------
double rhoc = 1450.00; // density of the cable // [ kg/m^3]
double cc = 1.00; // frictional constant // [ ]
double dc = 0.05614; // diameter // [ m ]
// PARAMETERS OF THE WIND :
// -----------------------------
double w0 = 10.00; // wind velocity at altitude h0 // [ m/s ]
double h0 = 100.00; // the altitude h0 // [ m ]
double hr = 0.10; // roughness length // [ m ]
// OTHER VARIABLES :
// ------------------------
double AQ ; // cross sectional area
IntermediateState mc; // mass of the cable
IntermediateState m ; // effective inertial mass
IntermediateState m_; // effective gravitational mass
IntermediateState Cg;
IntermediateState dm; // first derivative of m with respect to t
// DEFINITION OF PI :
// ------------------------
double PI = 3.1415926535897932;
// ORIENTATION AND FORCES :
// ------------------------
IntermediateState h ; // altitude of the kite
IntermediateState w ; // the wind at altitude h
IntermediateState we [ 3]; // effective wind vector
IntermediateState nwe ; // norm of the effective wind vector
IntermediateState nwep ; // -
IntermediateState ewep [ 3]; // projection of ewe
IntermediateState eta ; // angle eta: cf. [2]
IntermediateState et [ 3]; // unit vector from the left to the right wing tip
IntermediateState Caer ;
IntermediateState Cf ; // simple constants
IntermediateState CD ; // the aerodynamic drag coefficient
IntermediateState Fg [ 3]; // the gravitational force
IntermediateState Faer [ 3]; // the aerodynamic force
IntermediateState Ff [ 3]; // the frictional force
IntermediateState F [ 3]; // the total force
// TERMS ON RIGHT-HAND-SIDE
// OF THE DIFFERENTIAL
// EQUATIONS :
// ------------------------
IntermediateState a_pseudo [ 3]; // the pseudo accelaration
IntermediateState dn ; // the time derivate of the kite's winding number
IntermediateState ddr ; // second derivative of s with respect to t
IntermediateState ddphi ; // second derivative of phi with respect to t
IntermediateState ddtheta ; // second derivative of theta with respect to t
IntermediateState power ; // the power at the generator
// ------------------------ ------ // ----------------------------------------------
IntermediateState regularisation ; // regularisation of controls
// MODEL EQUATIONS :
// ===============================================================
// SPRING CONSTANT OF THE CABLE :
// ---------------------------------------------------------------
AQ = PI*dc*dc/4.0 ;
// THE EFECTIVE MASS' :
// ---------------------------------------------------------------
mc = rhoc*AQ*r ; // mass of the cable
m = mk + mc / 3.0; // effective inertial mass
m_ = mk + mc / 2.0; // effective gravitational mass
// ----------------------------- // ----------------------------
dm = (rhoc*AQ/ 3.0)*dr; // time derivative of the mass
// WIND SHEAR MODEL :
// ---------------------------------------------------------------
// for startup +60m
h = r*cos(theta)+60.0 ;
// h = r*cos(theta) ;
w = log(h/hr) / log(h0/hr) *(w0+w_extra) ;
// EFFECTIVE WIND IN THE KITE`S SYSTEM :
// ---------------------------------------------------------------
we[0] = w*sin(theta)*cos(phi) - dr ;
we[1] = -w*sin(phi) - r*sin(theta)*dphi ;
we[2] = -w*cos(theta)*cos(phi) + r *dtheta;
// CALCULATION OF THE KITE`S TRANSVERSAL AXIS :
// ---------------------------------------------------------------
nwep = pow( we[1]*we[1] + we[2]*we[2], 0.5 );
nwe = pow( we[0]*we[0] + we[1]*we[1] + we[2]*we[2], 0.5 );
eta = asin( we[0]*tan(Psi)/ nwep ) ;
// ---------------------------------------------------------------
// ewep[0] = 0.0 ;
ewep[1] = we[1] / nwep ;
ewep[2] = we[2] / nwep ;
// ---------------------------------------------------------------
et [0] = sin(Psi) ;
et [1] = (-cos(Psi)*sin(eta))*ewep[1] - (cos(Psi)*cos(eta))*ewep[2];
et [2] = (-cos(Psi)*sin(eta))*ewep[2] + (cos(Psi)*cos(eta))*ewep[1];
// CONSTANTS FOR SEVERAL FORCES :
// ---------------------------------------------------------------
Cg = (V*rho-m_)*g ;
Caer = (rho*A/2.0 )*nwe ;
Cf = (rho*dc/8.0)*r*nwe ;
// THE DRAG-COEFFICIENT :
// ---------------------------------------------------------------
CD = cd0 + K*CL*CL ;
// SUM OF GRAVITATIONAL AND LIFTING FORCE :
// ---------------------------------------------------------------
Fg [0] = Cg * cos(theta) ;
// Fg [1] = Cg * 0.0 ;
Fg [2] = Cg * sin(theta) ;
// SUM OF THE AERODYNAMIC FORCES :
// ---------------------------------------------------------------
Faer[0] = Caer*( CL*(we[1]*et[2]-we[2]*et[1]) + CD*we[0] ) ;
Faer[1] = Caer*( CL*(we[2]*et[0]-we[0]*et[2]) + CD*we[1] ) ;
Faer[2] = Caer*( CL*(we[0]*et[1]-we[1]*et[0]) + CD*we[2] ) ;
// THE FRICTION OF THE CABLE :
// ---------------------------------------------------------------
// Ff [0] = Cf * cc* we[0] ;
Ff [1] = Cf * cc* we[1] ;
Ff [2] = Cf * cc* we[2] ;
// THE TOTAL FORCE :
// ---------------------------------------------------------------
F [0] = Fg[0] + Faer[0] ;
F [1] = Faer[1] + Ff[1] ;
F [2] = Fg[2] + Faer[2] + Ff[2] ;
// THE PSEUDO ACCELERATION:
// ---------------------------------------------------------------
a_pseudo [0] = - ddr0
+ r*( dtheta*dtheta
+ sin(theta)*sin(theta)*dphi *dphi )
- dm/m*dr ;
a_pseudo [1] = - 2.0*cos(theta)/sin(theta)*dphi*dtheta
- 2.0*dr/r*dphi
- dm/m*dphi ;
a_pseudo [2] = cos(theta)*sin(theta)*dphi*dphi
- 2.0*dr/r*dtheta
- dm/m*dtheta ;
// THE EQUATIONS OF MOTION:
// ---------------------------------------------------------------
ddr = F[0]/m + a_pseudo[0] ;
ddphi = F[1]/(m*r*sin(theta)) + a_pseudo[1] ;
ddtheta = -F[2]/(m*r ) + a_pseudo[2] ;
// THE DERIVATIVE OF THE WINDING NUMBER :
// ---------------------------------------------------------------
dn = ( dphi*ddtheta - dtheta*ddphi ) /
(2.0*PI*(dphi*dphi + dtheta*dtheta) ) ;
// THE POWER AT THE GENERATOR :
// ---------------------------------------------------------------
power = m*ddr*dr ;
// REGULARISATION TERMS :
// ---------------------------------------------------------------
regularisation = 5.0e2 * ddr0 * ddr0
+ 1.0e8 * dPsi * dPsi
+ 1.0e5 * dCL * dCL
+ 2.5e5 * dn * dn
+ 2.5e7 * ddphi * ddphi;
+ 2.5e7 * ddtheta * ddtheta;
+ 2.5e6 * dtheta * dtheta;
// ---------------------------
// REFERENCE TRAJECTORY:
// ---------------------------------------------------------------
VariablesGrid myReference; myReference.read( "ref_w_zeros.txt" );// read the measurements
PeriodicReferenceTrajectory reference( myReference );
// THE "RIGHT-HAND-SIDE" OF THE ODE:
// ---------------------------------------------------------------
DifferentialEquation f;
f << dot(r) == dr ;
f << dot(phi) == dphi ;
f << dot(theta) == dtheta ;
f << dot(dr) == ddr0 ;
f << dot(dphi) == ddphi ;
f << dot(dtheta) == ddtheta ;
f << dot(n) == dn ;
f << dot(Psi) == dPsi ;
f << dot(CL) == dCL ;
f << dot(W) == (-power + regularisation)*1.0e-6;
model_response << r ; // the state r is measured
model_response << phi ;
model_response << theta;
model_response << dr ;
model_response << dphi ;
model_response << dtheta;
model_response << ddr0;
model_response << dPsi;
model_response << dCL ;
DVector x_scal(9);
x_scal(0) = 60.0;
x_scal(1) = 1.0e-1;
x_scal(2) = 1.0e-1;
x_scal(3) = 40.0;
x_scal(4) = 1.0e-1;
x_scal(5) = 1.0e-1;
x_scal(6) = 60.0;
x_scal(7) = 1.5e-1;
x_scal(8) = 2.5;
DMatrix Q(9,9);
Q.setIdentity();
DMatrix Q_end(9,9);
Q_end.setIdentity();
int i;
for( i = 0; i < 6; i++ ){
Q(i,i) = (1.0e-1/x_scal(i))*(1.0e-1/x_scal(i));
Q_end(i,i) = (5.0e-1/x_scal(i))*(5.0e-1/x_scal(i));
}
for( i = 6; i < 9; i++ ){
Q(i,i) = (1.0e-1/x_scal(i))*(1.0e-1/x_scal(i));
Q_end(i,i) = (5.0e-1/x_scal(i))*(5.0e-1/x_scal(i));
}
DVector measurements(9);
measurements.setAll( 0.0 );
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
const double t_start = 0.0;
const double t_end = 10.0;
OCP ocp( t_start, t_end, 10 );
ocp.minimizeLSQ( Q,model_response, measurements ) ; // fit h to the data
ocp.minimizeLSQEndTerm( Q_end,model_response, measurements ) ;
ocp.subjectTo( f );
ocp.subjectTo( -0.34 <= phi <= 0.34 );
ocp.subjectTo( 0.85 <= theta <= 1.45 );
ocp.subjectTo( -40.0 <= dr <= 10.0 );
ocp.subjectTo( -0.29 <= Psi <= 0.29 );
ocp.subjectTo( 0.1 <= CL <= 1.50 );
ocp.subjectTo( -0.7 <= n <= 0.90 );
ocp.subjectTo( -25.0 <= ddr0 <= 25.0 );
ocp.subjectTo( -0.065 <= dPsi <= 0.065 );
ocp.subjectTo( -3.5 <= dCL <= 3.5 );
ocp.subjectTo( -60.0 <= cos(theta)*r );
ocp.subjectTo( w_extra == 0.0 );
// SETTING UP THE (SIMULATED) PROCESS:
// -----------------------------------
OutputFcn identity;
DynamicSystem dynamicSystem( f,identity );
Process process( dynamicSystem,INT_RK45 );
VariablesGrid disturbance; disturbance.read( "my_wind_disturbance_controlsfree.txt" );
if (process.setProcessDisturbance( disturbance ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
// SETUP OF THE ALGORITHM AND THE TUNING OPTIONS:
// ----------------------------------------------
double samplingTime = 1.0;
RealTimeAlgorithm algorithm( ocp, samplingTime );
if (algorithm.initializeDifferentialStates("p_s_ref.txt" ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
if (algorithm.initializeControls ("p_c_ref.txt" ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
algorithm.set( MAX_NUM_ITERATIONS, 2 );
algorithm.set( KKT_TOLERANCE , 1e-4 );
algorithm.set( HESSIAN_APPROXIMATION,GAUSS_NEWTON);
algorithm.set( INTEGRATOR_TOLERANCE, 1e-6 );
algorithm.set( GLOBALIZATION_STRATEGY,GS_FULLSTEP );
// algorithm.set( USE_IMMEDIATE_FEEDBACK, YES );
algorithm.set( USE_REALTIME_SHIFTS, YES );
algorithm.set(LEVENBERG_MARQUARDT, 1e-5);
DVector x0(10);
x0(0) = 1.8264164528775887e+03;
x0(1) = -5.1770453309520573e-03;
x0(2) = 1.2706440287266794e+00;
x0(3) = 2.1977888424944396e+00;
x0(4) = 3.1840786108641383e-03;
x0(5) = -3.8281200674676448e-02;
x0(6) = 0.0000000000000000e+00;
x0(7) = -1.0372313936413566e-02;
x0(8) = 1.4999999999999616e+00;
x0(9) = 0.0000000000000000e+00;
// SETTING UP THE NMPC CONTROLLER:
// -------------------------------
Controller controller( algorithm, reference );
// SETTING UP THE SIMULATION ENVIRONMENT, RUN THE EXAMPLE...
// ----------------------------------------------------------
double simulationStart = 0.0;
double simulationEnd = 10.0;
SimulationEnvironment sim( simulationStart, simulationEnd, process, controller );
if (sim.init( x0 ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
if (sim.run( ) != SUCCESSFUL_RETURN)
exit( EXIT_FAILURE );
// ...AND PLOT THE RESULTS
// ----------------------------------------------------------
VariablesGrid diffStates;
sim.getProcessDifferentialStates( diffStates );
diffStates.print( "diffStates.txt" );
diffStates.print( "diffStates.m","DIFFSTATES",PS_MATLAB );
VariablesGrid interStates;
sim.getProcessIntermediateStates( interStates );
interStates.print( "interStates.txt" );
interStates.print( "interStates.m","INTERSTATES",PS_MATLAB );
VariablesGrid sampledProcessOutput;
sim.getSampledProcessOutput( sampledProcessOutput );
sampledProcessOutput.print( "sampledOut.txt" );
sampledProcessOutput.print( "sampledOut.m","OUT",PS_MATLAB );
VariablesGrid feedbackControl;
sim.getFeedbackControl( feedbackControl );
feedbackControl.print( "controls.txt" );
feedbackControl.print( "controls.m","CONTROL",PS_MATLAB );
GnuplotWindow window;
window.addSubplot( sampledProcessOutput(0), "DIFFERENTIAL STATE: r" );
window.addSubplot( sampledProcessOutput(1), "DIFFERENTIAL STATE: phi" );
window.addSubplot( sampledProcessOutput(2), "DIFFERENTIAL STATE: theta" );
window.addSubplot( sampledProcessOutput(3), "DIFFERENTIAL STATE: dr" );
window.addSubplot( sampledProcessOutput(4), "DIFFERENTIAL STATE: dphi" );
window.addSubplot( sampledProcessOutput(5), "DIFFERENTIAL STATE: dtheta" );
window.addSubplot( sampledProcessOutput(7), "DIFFERENTIAL STATE: Psi" );
window.addSubplot( sampledProcessOutput(8), "DIFFERENTIAL STATE: CL" );
window.addSubplot( sampledProcessOutput(9), "DIFFERENTIAL STATE: W" );
window.addSubplot( feedbackControl(0), "CONTROL 1 DDR0" );
window.addSubplot( feedbackControl(1), "CONTROL 1 DPSI" );
window.addSubplot( feedbackControl(2), "CONTROL 1 DCL" );
window.plot( );
GnuplotWindow window2;
window2.addSubplot( interStates(1) );
window2.plot();
return 0;
}
