initial

Project.toml

@@ -11,6 +11,7 @@ Distributions = "31c24e10-a181-5473-b8eb-7969acd0382f"
 ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210"
 KernelDensity = "5ab0869b-81aa-558d-bb23-cbf5423bbe9b"
 Libtask = "6f1fad26-d15e-5dc8-ae53-837a1d7b8c9f"
+LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 ParameterizedFunctions = "65888b18-ceab-5e60-b2b9-181511a3b968"
 Parameters = "d96e819e-fc66-5662-9728-84c9c7592b0a"
 Turing = "fce5fe82-541a-59a6-adf8-730c64b5f9a0"

src/ShipMMG.jl

@@ -7,6 +7,7 @@ using Parameters
 using Distributions
 using Turing
 using ForwardDiff
+using LinearAlgebra
 
 include("data.jl")
 export calc_position,

src/mmg.jl

@@ -1181,5 +1181,296 @@ function create_model_for_mcmc_sample_mmg(
         end
     end
 
+    return fitMMG(time_obs, [u_obs, v_obs, r_obs], prob1)
+end
+
+function create_model_for_mcmc_sample_mmg(
+    data::ShipData,
+    basic_params::Mmg3DofBasicParams,
+    k_0,
+    k_1,
+    k_2,
+    multivariate_prior_dist;
+    ρ=1025.0,
+    σ_u_prior_dist=Uniform(0.00, 0.20),
+    σ_v_prior_dist=Uniform(0.00, 0.20),
+    σ_r_prior_dist=Uniform(0.00, 0.20),
+    solver=Tsit5(),
+    abstol=1e-6,
+    reltol=1e-3
+)
+    time_obs = data.time
+    u_obs = data.u
+    v_obs = data.v
+    r_obs = data.r
+    δ_obs = data.δ
+    n_p_obs = data.n_p
+
+    @unpack L_pp,
+    B,
+    d,
+    x_G,
+    D_p,
+    m,
+    I_zG,
+    A_R,
+    η,
+    m_x,
+    m_y,
+    J_z,
+    f_α,
+    ϵ,
+    t_R,
+    x_R,
+    a_H,
+    x_H,
+    γ_R_minus,
+    γ_R_plus,
+    l_R,
+    κ,
+    t_P,
+    w_P0,
+    x_P = basic_params
+
+    # create sytem model
+    spl_δ = Spline1D(time_obs, δ_obs)
+    spl_n_p = Spline1D(time_obs, n_p_obs)
+    function MMG!(dX, X, p, t)
+        u, v, r, δ, n_p = X
+        R_0_dash,
+        X_vv_dash,
+        X_vr_dash,
+        X_rr_dash,
+        X_vvvv_dash,
+        Y_v_dash,
+        Y_r_dash,
+        Y_vvv_dash,
+        Y_vvr_dash,
+        Y_vrr_dash,
+        Y_rrr_dash,
+        N_v_dash,
+        N_r_dash,
+        N_vvv_dash,
+        N_vvr_dash,
+        N_vrr_dash,
+        N_rrr_dash = p
+
+        U = sqrt(u^2 + (v - r * x_G)^2)
+
+        β = 0.0
+        if U == 0.0
+            β = 0.0
+        else
+            β = asin(-(v - r * x_G) / U)
+        end
+
+        v_dash = 0.0
+        if U == 0.0
+            v_dash = 0.0
+        else
+            v_dash = v / U
+        end
+
+        r_dash = 0.0
+        if U == 0.0
+            r_dash = 0.0
+        else
+            r_dash = r * L_pp / U
+        end
+
+        w_P = w_P0 * exp(-4.0 * (β - x_P * r_dash)^2)
+
+        J = 0.0
+        if n_p == 0.0
+            J = 0.0
+        else
+            J = (1 - w_P) * u / (n_p * D_p)
+        end
+        K_T = k_0 + k_1 * J + k_2 * J^2
+        β_R = β - l_R * r_dash
+        γ_R = γ_R_minus
+
+        if β_R < 0.0
+            γ_R = γ_R_minus
+        else
+            γ_R = γ_R_plus
+        end
+
+        v_R = U * γ_R * β_R
+
+        u_R = 0.0
+        if J == 0.0
+            u_R = sqrt(η * (κ * ϵ * 8.0 * k_0 * n_p^2 * D_p^4 / pi)^2)
+        else
+            u_R =
+                u *
+                (1.0 - w_P) *
+                ϵ *
+                sqrt(η * (1.0 + κ * (sqrt(1.0 + 8.0 * K_T / (pi * J^2)) - 1))^2 + (1 - η))
+        end
+
+        U_R = sqrt(u_R^2 + v_R^2)
+        α_R = δ - atan(v_R, u_R)
+        F_N = 0.5 * A_R * ρ * f_α * (U_R^2) * sin(α_R)
+
+        X_H = (
+            0.5 *
+            ρ *
+            L_pp *
+            d *
+            (U^2) *
+            (
+                -R_0_dash +
+                X_vv_dash * (v_dash^2) +
+                X_vr_dash * v_dash * r_dash +
+                X_rr_dash * (r_dash^2) +
+                X_vvvv_dash * (v_dash^4)
+            )
+        )
+        X_R = -(1.0 - t_R) * F_N * sin(δ)
+        X_P = (1.0 - t_P) * ρ * K_T * n_p^2 * D_p^4
+        Y_H = (
+            0.5 *
+            ρ *
+            L_pp *
+            d *
+            (U^2) *
+            (
+                Y_v_dash * v_dash +
+                Y_r_dash * r_dash +
+                Y_vvv_dash * (v_dash^3) +
+                Y_vvr_dash * (v_dash^2) * r_dash +
+                Y_vrr_dash * v_dash * (r_dash^2) +
+                Y_rrr_dash * (r_dash^3)
+            )
+        )
+        Y_R = -(1 + a_H) * F_N * cos(δ)
+        N_H = (
+            0.5 *
+            ρ *
+            (L_pp^2) *
+            d *
+            (U^2) *
+            (
+                N_v_dash * v_dash +
+                N_r_dash * r_dash +
+                N_vvv_dash * (v_dash^3) +
+                N_vvr_dash * (v_dash^2) * r_dash +
+                N_vrr_dash * v_dash * (r_dash^2) +
+                N_rrr_dash * (r_dash^3)
+            )
+        )
+        N_R = -(x_R + a_H * x_H) * F_N * cos(δ)
+        dX[1] = du = ((X_H + X_R + X_P) + (m + m_y) * v * r + x_G * m * (r^2)) / (m + m_x)
+        dX[2] =
+            dv =
+                (
+                    (x_G^2) * (m^2) * u * r - (N_H + N_R) * x_G * m +
+                    ((Y_H + Y_R) - (m + m_x) * u * r) * (I_zG + J_z + (x_G^2) * m)
+                ) / ((I_zG + J_z + (x_G^2) * m) * (m + m_y) - (x_G^2) * (m^2))
+        dX[3] = dr = (N_H + N_R - x_G * m * (dv + u * r)) / (I_zG + J_z + (x_G^2) * m)
+        dX[4] = dδ = derivative(spl_δ, t)
+        dX[5] = dn_p = derivative(spl_n_p, t)
+    end
+
+    R_0_dash_start = 0.022
+    X_vv_dash_start = -0.040
+    X_vr_dash_start = 0.002
+    X_rr_dash_start = 0.011
+    X_vvvv_dash_start = 0.771
+    Y_v_dash_start = -0.315
+    Y_r_dash_start = 0.083
+    Y_vvv_dash_start = -1.607
+    Y_vvr_dash_start = 0.379
+    Y_vrr_dash_start = -0.391
+    Y_rrr_dash_start = 0.008
+    N_v_dash_start = -0.137
+    N_r_dash_start = -0.049
+    N_vvv_dash_start = -0.030
+    N_vvr_dash_start = -0.294
+    N_vrr_dash_start = 0.055
+    N_rrr_dash_start = -0.013
+
+    p = [
+        R_0_dash_start,
+        X_vv_dash_start,
+        X_vr_dash_start,
+        X_rr_dash_start,
+        X_vvvv_dash_start,
+        Y_v_dash_start,
+        Y_r_dash_start,
+        Y_vvv_dash_start,
+        Y_vvr_dash_start,
+        Y_vrr_dash_start,
+        Y_rrr_dash_start,
+        N_v_dash_start,
+        N_r_dash_start,
+        N_vvv_dash_start,
+        N_vvr_dash_start,
+        N_vrr_dash_start,
+        N_rrr_dash_start,
+    ]
+
+    u0 = 2.29 * 0.512
+    v0 = 0.0
+    r0 = 0.0
+    X0 = [u_obs[1]; v_obs[1]; r_obs[1]; δ_obs[1]; n_p_obs[1]]
+    prob1 = ODEProblem(MMG!, X0, (time_obs[1], time_obs[end]), p)
+
+    # create probabilistic model
+    @model function fitMMG(time_obs, obs, prob1)
+        σ_u ~ σ_u_prior_dist
+        σ_v ~ σ_v_prior_dist
+        σ_r ~ σ_r_prior_dist
+        p ~ multivariate_prior_dist;
+
+        # パラメーターの分解
+        R_0_dash,
+        X_vv_dash,
+        X_vr_dash,
+        X_rr_dash,
+        X_vvvv_dash,
+        Y_v_dash,
+        Y_r_dash,
+        Y_vvv_dash,
+        Y_vvr_dash,
+        Y_vrr_dash,
+        Y_rrr_dash,
+        N_v_dash,
+        N_r_dash,
+        N_vvv_dash,
+        N_vvr_dash,
+        N_vrr_dash,
+        N_rrr_dash = p
+
+        p_array = [
+            R_0_dash,
+            X_vv_dash,
+            X_vr_dash,
+            X_rr_dash,
+            X_vvvv_dash,
+            Y_v_dash,
+            Y_r_dash,
+            Y_vvv_dash,
+            Y_vvr_dash,
+            Y_vrr_dash,
+            Y_rrr_dash,
+            N_v_dash,
+            N_r_dash,
+            N_vvv_dash,
+            N_vvr_dash,
+            N_vrr_dash,
+            N_rrr_dash
+        ]
+        prob = remake(prob1, p=p_array)
+        sol = solve(prob, solver, abstol=abstol, reltol=reltol)
+        predicted = sol(time_obs)
+        for i in eachindex(predicted)
+            obs[1][i] ~ Normal(predicted[i][1], σ_u) # u
+            obs[2][i] ~ Normal(predicted[i][2], σ_v) # v
+            obs[3][i] ~ Normal(predicted[i][3], σ_r) # r
+        end
+    end
+
     return fitMMG(time_obs, [u_obs, v_obs, r_obs], prob1)
 end

test/runtests.jl

@@ -1,6 +1,7 @@
 using ShipMMG
 using Test
 using Distributions
+using LinearAlgebra
 
 @testset "ShipMMG.jl" begin
     include("test_data.jl")