float Ship::KGh(){
float hulltotal = Wh() + Wm() + Wwo() + We() + Wspus() + Bunker();
float WKGh = (Wh() + Wwo()) * KGs();
float WKGe = We() * KGe();
float WKGm = (Wm() + Bunker()) * KGm();
float WKGspus = Wspus() * KGspus();
float armortot = Wmb() + Web() + Wub() + Wmd() + Wwd() + Wsd() + Wed() + Wblk();
float WKGmb = Wmb() * KGmb();
float WKGeb = Web() * KGeb();
float WKGub = Wub() * KGub();
float WKGmd = Wmd() * KGmd();
float WKGwd = Wwd() * KGwd();
float WKGsd = Wsd() * KGsd();
float WKGed = Wed() * KGed();
float WKGblk = Wblk() * KGblk();
float total = hulltotal + armortot;
return (WKGh + WKGe + WKGm + WKGspus + WKGmb + WKGeb + WKGub + WKGmd + WKGwd + WKGsd + WKGed + WKGblk) / total;
}
/// \brief Constructor: Initializes filter
UnscentedKalmanFilter::UnscentedKalmanFilter(void):
x_hat(N,0.0),P_hat(N,N,0.0),K(N,M,0.0),Q(N,N,0.0),R(M,M,0.0),Wm(1,2*N+1,0.5/C),Wc(1,2*N+1,0.5/C)
{
// Set mean of initial position prior (orientation already 0)
x_hat[0] = 0.0; x_hat[1] = 40.0; x_hat[2] = -15.0;
x_hat[3] = 0.0; x_hat[4] = 0.0; x_hat[5] = 0.0;
// Set diagonal covariance entries for prior
P_hat.fill_diagonal(P_POS_INIT);
P_hat(3,3) = P_ROT_INIT; P_hat(4,4) = P_ROT_INIT; P_hat(5,5) = P_ROT_INIT;
// Set diagonal covariance entries for process noise
Q.fill_diagonal(Q_POS);
Q(3,3) = Q_ROT; Q(4,4) = Q_ROT; Q(5,5) = Q_ROT;
// Set diagonal covariance entries for measurement noise
R.fill_diagonal(R_POS);
R(3,3) = R_ROT; R(4,4) = R_ROT; R(5,5) = R_ROT;
// Set weighting for UKF sigma points
Wm(0,0) = (LAMBDA)/(C);
Wc = Wm;
double temp = Wc(0,0);
Wc(0,0) = temp + 1-pow(ALPHA,2)+BETA;
}
void BFilterUKF::predict(){
//L=numel(x); //numer of states
//m=numel(z); //numer of measurements
unsigned int numStates = particles.samples.n_cols;
float alpha=1.0f; //default 1e-3, tunable
float kappa=0.0f; //default, tunable
float beta=2.0f; //default, tunable
//lambda=alpha^2*(L+kappa)-L; //scaling factor
float lambda=alpha*alpha*(numStates+kappa)-numStates; //scaling factor
float c=numStates+lambda; //scaling factor gamma
//Wm=[lambda/c 0.5/c+zeros(1,2*L)]; //weights for means
Wm=zeros<fmat>(1, 2*numStates+1) + (0.5f/c); //weights for means
Wm(0)=lambda/c;
Wc=Wm;
Wc(0)=Wc(0)+(1-alpha*alpha+beta); //weights for covariance
c=sqrt(c);
fmat X=sigmas(fvec(particles.samples),P,c); //sigma points around x
utProcess(X,Wm,Wc,numStates,process->Q);
}
float Ship::Hullship(){
return Wh() + Wwo() + Wm() + We() + Wspus() + Bunker();
}
void Ship::output(){
ofstream file;
file.open("Report.txt");
file << "[table]" << endl;
cout << fixed << setprecision(2) << "Length: \t\t\t\t" << v.Lpp << " m" << endl;
file << fixed << setprecision(2) << "[tr][td]Length: \t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t\t[/td][td]" << v.Lpp << " m[/td][/tr]" << endl;
cout << "Beam: \t\t\t\t\t" << v.B << " m" << endl;
file << "[tr][td]Beam: [/td][td]" << v.B << " m[/td][/tr]" << endl;
cout << "Draft: \t\t\t\t\t" << v.D << " m" << endl;
file << "[tr][td]Draft: [/td][td]" << v.D << " m[/td][/tr]" << endl;
cout << "Freeboard: \t\t\t\t" << v.fB << " m" << endl;
file << "[tr][td]Freeboard: [/td][td]" << v.fB << " m[/td][/tr]" << endl;
cout << setprecision(3) << "Block Coefficient: \t\t\t" << Cb() << endl;
file << setprecision(3) << "[tr][td]Block Coefficient: [/td][td]" << Cb() << "[/td][/tr]" << endl;
cout << setprecision(0) << "Displacement: \t\t\t\t" << Vol() << " t" << endl;
file << setprecision(0) << "[tr][td]Displacement: [/td][td]" << Vol() << " t[/td][/tr]" << endl;
cout << "Lightship: \t\t\t\t" << Wh() + Wwo() + Wm() + We() + Wspus() + Bunker() << " t" << endl;
file << "[tr][td]Lightship: [/td][td]" << Lightship() << " t[/td][/tr]" << endl;
cout << setprecision(1) << "\nMax Speed: \t\t\t\t" << v.Vk << " kn" << endl;
file << setprecision(1) << "\n[tr][td]Max Speed: [/td][td]" << v.Vk << " kn[/td][/tr]" << endl;
cout << "Cruise Speed: \t\t\t\t" << v.Vc << " kn / " << v.Range << " nm" << endl;
file << "[tr][td]Cruise Speed: [/td][td]" << v.Vc << " kn / " << v.Range << " nm[/td][/tr]" << endl;
cout << "Engine: \t\t\t\t";
file << "[tr][td]Engine: [/td][td]";
switch(e.Gear){
case Direct:
cout << "Direct ";
file << "Direct ";
break;
case Geared:
cout << "Geared ";
file << "Geared ";
break;
case Turbo_Electric:
cout << "Turbo-Electric ";
file << "Turbo-Electric ";
break;
default:
cout << "Unknown";
file << "Unknown";
}
switch(e.Eng){
case Diesel2stk:
cout << "Two Stroke Diesel" << endl;
file << "Two Stroke Diesel";
break;
case Diesel4stk:
cout << "Four Stroke Diesel" << endl;
file << "Four Stroke Diesel" << endl;
break;
case QuadExp:
cout << "Quadruple Expansion Reciprocating Engine" << endl;
file << "Quadruple Expansion Reciprocating Engine";
break;
case TripExp:
cout << "Triple Expansion Reciprocating Engine" << endl;
file << "Triple Expansion Reciprocating Engine";
break;
case SimExp:
cout << "Simple Reciprocating Engine" << endl;
file << "Simple Reciprocating Engine";
break;
case SteamTur:
cout << "Steam Turbine" << endl;
file << "Steam Turbine";
break;
default:
cout << endl;
}
file << "[/td][/tr]" << endl;
cout << setprecision(0) << "Power Delivered: \t\t\t" << Pd(v.Vk) << " hp" << endl;
file << setprecision(0) << "[tr][td]Power Delivered: [/td][td]" << Pd(v.Vk) << " hp[/td][/tr]" << endl;
cout << setprecision(2) << "Total Efficiency: \t\t\t" << NUt()*100 << "%" << endl;
file << setprecision(2) << "[tr][td]Total Efficiency: [/td][td]" << NUt()*100 << "%[/td][/tr]" << endl;
cout << setprecision(3) << "Froude Number: \t\t\t\t" << Fn() << endl;
file << setprecision(3) << "[tr][td]Froude Number: [/td][td]" << Fn() << "[/td][/tr]" << endl;
cout << setprecision(0) << "Bunker Size: \t\t\t\t" << Bunker() << " t" << endl;
file << setprecision(0) << "[tr][td]Bunker Size: [/td][td]" << Bunker() << " t[/td][/tr]" << endl;
cout << "Service Allowance: \t\t\t" << e.SA << "%" << endl;
file << "[tr][td]Service Allowance: [/td][td]" << e.SA << "%[/td][/tr]" << endl;
cout << setprecision(2);
file << setprecision(2);
cout << "Length of Superstructure: \t\t" << v.Lspus << " m" << endl;
file << "[tr][td]Length of Superstructure: [/td][td]" << v.Lspus << " m[/td][/tr]" << endl;
cout << "Aftbody Shape: \t\t\t\t";
file << "[tr][td]Aftbody Shape: [/td][td]";
switch(e.Faa){
case V:
cout << "V" << endl;
file << "V";
break;
case U:
cout << "U" << endl;
file << "U";
break;
case N:
cout << "N" << endl;
file << "N";
break;
default:
cout << "N" << endl;
file << "N";
}
file << "[/td][/tr]";
cout << "\nLongitudinal Center of Buoyancy: \t" << lcb() << "%" << endl;
file << "\n[tr][td]Longitudinal Center of Buoyancy: [/td][td]" << lcb() << "% / " << lcb()*v.Lpp << " m from midpoint[/td][/tr]" << endl;
cout << "Longitudinal Center of Gravity: \t" << lcg() << "%" << endl;
file << "[tr][td]Longitudinal Center of Gravity: [/td][td]" << lcg() << "% / " << lcg()*v.Lpp << " m from midpoint[/td][/tr]" << endl;
cout << "Vertical Center of Gravity: \t\t" << KGh() << " m" << endl;
file << "[tr][td]Vertical Center of Gravity: [/td][td]" << KGh() << " m[/td][/tr]" << endl;
cout << "Metacentric Height: \t\t\t" << GM() << " m" << endl;
file << "[tr][td]Metacentric Height: [/td][td]" << GM() << " m[/td][/tr]" << endl;
cout << "Roll Period: \t\t\t\t" << TR() << " s" << endl;
file << "[tr][td]Roll Period: [/td][td]" << TR() << " s[/td][/tr]" << endl;
cout << "\nBow Entrance Angle: \t\t\t" << iE() << " deg" << endl;
file << "\n[tr][td]Bow Entrance Angle: [/td][td]" << iE() << " deg[/td][/tr]" << endl;
cout << "Length of Engine Room: \t\t\t" << Lcm() << " m" << endl;
file << "[tr][td]Length of Engine Room: [/td][td]" << Lcm() << " m[/td][/tr]" << endl;
cout << setprecision(0) << "\nMain Belt: \t\t\t\t" << arm.mb_Ttop << " / " << arm.mb_Tbot << " mm at " << arm.b_deg << " degrees, " << arm.b_L << " m long with " << arm.b_H << " m above and " << arm.b_Db << " m below water" << endl;
file << setprecision(0) << "\n[tr][td]Main Belt: [/td][td]" << arm.mb_Ttop << " / " << arm.mb_Tbot << " mm, " << arm.b_L << " m long with " << arm.b_H << " m above and " << arm.b_Db << " m below water[/td][/tr]" << endl;
cout << "End Belt: \t\t\t\t" << arm.eb_Ttop << " / " << arm.eb_Tbot << " mm" << endl;
file << "[tr][td]End Belt: [/td][td]" << arm.eb_Ttop << " / " << arm.eb_Tbot << " mm[/td][/tr]" << endl;
cout << "Upper Belt: \t\t\t\t" << arm.ub_Ttop << " / " << arm.ub_Tbot << " mm, " << arm.b_uL << " m long" << endl;
file << "[tr][td]Upper Belt: [/td][td]" << arm.ub_Ttop << " / " << arm.ub_Tbot << " mm, " << arm.b_uL << " m long[/td][/tr]" << endl;
cout << "Main Deck: \t\t\t\t" << arm.md_T << " mm covering " << arm.md_P << "% / " << arm.md_P * v.Lpp << " m of deck" << endl;
file << "[tr][td]Main Deck: [/td][td]" << arm.md_T << " m covering " << arm.md_P << "% / " << arm.md_P * v.Lpp << " m of deck[/td][/tr]" << endl;
cout << "Weather Deck: \t\t\t\t" << arm.wd_T << " mm covering " << arm.wd_P << "% / " << arm.wd_P * v.Lpp << " m of deck" << endl;
file << "[tr][td]Weather Deck: [/td][td]" << arm.wd_T << " m covering " << arm.wd_P << "% / " << arm.wd_P * v.Lpp << " m of deck[/td][/tr]" << endl;
cout << "Splinter Deck: \t\t\t\t" << arm.sd_T << " mm covering " << arm.sd_P << "% / " << arm.sd_P * v.Lpp << " m of deck" << endl;
file << "[tr][td]Splinter Deck: [/td][td]" << arm.sd_T << " m covering " << arm.sd_P << "% / " << arm.sd_P * v.Lpp << " m of deck[/td][/tr]" << endl;
cout << "Ends Deck: \t\t\t\t" << arm.ed_T << " mm" << endl;
file << "[tr][td]Ends Deck: [/td][td]" << arm.ed_T << " mm[/td][/tr]" << endl;
cout << "Bulkhead: \t\t\t\t" << arm.blk_T << " mm in " << arm.blk_Lct << "x" << arm.blk_D << " m layers, " << arm.blk_L << " m long, " << arm.blk_H << " m tall" << endl;
file << "[tr][td]Bulkhead: [/td][td]" << arm.blk_T << " mm in " << arm.blk_Lct << "x" << arm.blk_D << " m layers, " << arm.blk_L << " m long, " << arm.blk_H << " m tall[/td][/tr]" << endl;
file << "[/table]" << endl;
file.close();
}
float Ship::aB(){
return 0.9 * 68.7713 * exp(-2.18341 * v.Lpp * v.B / (Wh() + Wm()));
}
