diff --git a/.gitignore b/.gitignore
index 2171c4e43..0f1fd7edc 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,4 @@
+*.arrow
 *.bin
 *.xopp
 *.so
diff --git a/REUSE.toml b/REUSE.toml
index 125c39925..b1b3545a8 100644
--- a/REUSE.toml
+++ b/REUSE.toml
@@ -1,49 +1,31 @@
 version = 1
 
 [[annotations]]
-path = "data/prob*"
-
-SPDX-FileCopyrightText = "2025 Bart van de Lint"
-SPDX-License-Identifier = "MPL-2.0"
-
-[[annotations]]
-path = "data/ram_air_kite*"
-
+path = ["data/prob*", "data/ram*", "examples/discrete_jacobian.jl"]
 SPDX-FileCopyrightText = "2025 Bart van de Lint"
 SPDX-License-Identifier = "MPL-2.0"
 
 [[annotations]]
-path = "Manifest*"
-
-SPDX-FileCopyrightText = "2025 Uwe Fechner, Bart van de Lint"
-SPDX-License-Identifier = "MIT"
-
-[[annotations]]
-path = "Project.toml"
-
-SPDX-FileCopyrightText = "2025 Uwe Fechner, Bart van de Lint"
-SPDX-License-Identifier = "MIT"
+path = "data/*ram.yaml"
+SPDX-FileCopyrightText = "2025 Bart van de Lint, Uwe Fechner"
+SPDX-License-Identifier = "CC-BY-4.0"
 
 [[annotations]]
-path = "CITATION.cff"
-
+path = ["data/settings.yaml", "data/system.yaml", "data/vsm_settings.yaml"]
 SPDX-FileCopyrightText = "2025 Uwe Fechner"
-SPDX-License-Identifier = "MIT"
+SPDX-License-Identifier = "CC-BY-4.0"
 
 [[annotations]]
-path = ".gitignore"
-
+path = ["Manifest*", "Project.toml", ".gitignore"]
 SPDX-FileCopyrightText = "2025 Uwe Fechner, Bart van de Lint"
 SPDX-License-Identifier = "MIT"
 
 [[annotations]]
 path = ["docs/src/*.png", "docs/src/*.md", "docs/src/kite_power_tools.drawio"]
-
 SPDX-FileCopyrightText = "2025 Uwe Fechner"
 SPDX-License-Identifier = "CC-BY-4.0"
 
 [[annotations]]
-path = ["docs/.gitignore", "docs/Project.toml"]
-
+path = ["docs/.gitignore", "docs/Project.toml", "CITATION.cff"]
 SPDX-FileCopyrightText = "2025 Uwe Fechner"
 SPDX-License-Identifier = "MIT"
diff --git a/data/prob_1.10_ram_dynamic_3_seg.bin.default.xz b/data/prob_1.10_ram_dynamic_3_seg.bin.default.xz
index f64ae4cbd..dfb41fc02 100644
Binary files a/data/prob_1.10_ram_dynamic_3_seg.bin.default.xz and b/data/prob_1.10_ram_dynamic_3_seg.bin.default.xz differ
diff --git a/data/prob_1.11_ram_dynamic_3_seg.bin.default.xz b/data/prob_1.11_ram_dynamic_3_seg.bin.default.xz
index 99069f4b6..0252dfe6b 100644
Binary files a/data/prob_1.11_ram_dynamic_3_seg.bin.default.xz and b/data/prob_1.11_ram_dynamic_3_seg.bin.default.xz differ
diff --git a/data/settings.yaml.license b/data/settings.yaml.license
deleted file mode 100644
index 48d71b826..000000000
--- a/data/settings.yaml.license
+++ /dev/null
@@ -1,3 +0,0 @@
-SPDX-FileCopyrightText: 2025 Uwe Fechner
-
-SPDX-License-Identifier: CC-BY-4.0
diff --git a/data/settings_ram.yaml.license b/data/settings_ram.yaml.license
deleted file mode 100644
index 1036fe45c..000000000
--- a/data/settings_ram.yaml.license
+++ /dev/null
@@ -1,2 +0,0 @@
-SPDX-FileCopyrightText: 2025 Bart van de Lint
-SPDX-License-Identifier: CC-BY-4.0
diff --git a/data/system.yaml.license b/data/system.yaml.license
deleted file mode 100644
index 48d71b826..000000000
--- a/data/system.yaml.license
+++ /dev/null
@@ -1,3 +0,0 @@
-SPDX-FileCopyrightText: 2025 Uwe Fechner
-
-SPDX-License-Identifier: CC-BY-4.0
diff --git a/data/system_ram.yaml.license b/data/system_ram.yaml.license
deleted file mode 100644
index 74717d8f7..000000000
--- a/data/system_ram.yaml.license
+++ /dev/null
@@ -1,3 +0,0 @@
-SPDX-FileCopyrightText: 2025 Bart van de Lint
-
-SPDX-License-Identifier: CC-BY-4.0
diff --git a/data/vsm_settings.yaml.license b/data/vsm_settings.yaml.license
deleted file mode 100644
index 48d71b826..000000000
--- a/data/vsm_settings.yaml.license
+++ /dev/null
@@ -1,3 +0,0 @@
-SPDX-FileCopyrightText: 2025 Uwe Fechner
-
-SPDX-License-Identifier: CC-BY-4.0
diff --git a/docs/src/examples_ram_air.md b/docs/src/examples_ram_air.md
index ba51d09c3..c7850cc43 100644
--- a/docs/src/examples_ram_air.md
+++ b/docs/src/examples_ram_air.md
@@ -73,4 +73,15 @@ SIMPLE=true; include("examples/ram_air_kite.jl")
 The simple model has a very simple bridle system without pulleys and with less attachment points on the wing. 
 While the default model has a [speed system](https://kiteboarding.com/proddetail.asp?prod=ozone-r1v4-pro-tune-speedsystem-complete) with pulleys and more attachment points on the wing.
 
-![Oscillating steering input response, simple system](oscillating_steering_simple.png)
\ No newline at end of file
+![Oscillating steering input response, simple system](oscillating_steering_simple.png)
+
+## How to create a RamAirKite
+The following code is a minimal example that shows how to create a ram air kite struct:
+```
+using KiteModels
+
+# Initialize model
+set = load_settings("system_ram.yaml")
+
+rak = RamAirKite(set)
+```
\ No newline at end of file
diff --git a/docs/src/functions.md b/docs/src/functions.md
index 79fde76f0..73543ebef 100644
--- a/docs/src/functions.md
+++ b/docs/src/functions.md
@@ -68,7 +68,6 @@ calculate_rotational_inertia!
 calc_set_cl_cd!
 calc_aero_forces!
 calc_particle_forces!
-find_z_axis_point
 inner_loop!
 loop!
 ```
diff --git a/examples/ram_air_kite.jl b/examples/ram_air_kite.jl
index 345a6411e..af5302b65 100644
--- a/examples/ram_air_kite.jl
+++ b/examples/ram_air_kite.jl
@@ -26,7 +26,7 @@ include(joinpath(@__DIR__, "plotting.jl"))
 
 # Simulation parameters
 dt = 0.05
-total_time = 10  # Longer simulation to see oscillations
+total_time = 10.0  # Longer simulation to see oscillations
 vsm_interval = 3
 steps = Int(round(total_time / dt))
 
@@ -47,25 +47,25 @@ s.set.abs_tol = 1e-2
 s.set.rel_tol = 1e-2
 toc()
 
+measure = Measurement()
+measure.sphere_pos .= deg2rad.([70.0 70.0; 1.0 -1.0])
+
 # init_Q_b_w, R_b_w = KiteModels.measure_to_q(measure)
 # init_kite_pos = init!(s.point_system, s.set, R_b_w, init_Q_b_w)
-# plot(s.point_system, 0.0; zoom=false, front=true)
-
-measure = Measurement()
+# plot(s.point_system, 0.0; zoom=false, front=false)
 
 # Initialize at elevation
 s.point_system.winches[2].tether_length += 0.2
 s.point_system.winches[3].tether_length += 0.2
-measure.sphere_pos .= deg2rad.([70.0 70.0; 1.0 -1.0])
 KiteModels.init_sim!(s, measure; remake=false, reload=true)
 sys = s.sys
 
 @info "System initialized at:"
 toc()
 
-# # Stabilize system
+# Stabilize system
 s.integrator.ps[sys.stabilize] = true
-for i in 1:10÷dt
+for i in 1:1÷dt
     next_step!(s; dt, vsm_interval=1)
 end
 s.integrator.ps[sys.stabilize] = false
@@ -75,7 +75,8 @@ sys_state = KiteModels.SysState(s)
 t = 0.0
 runtime = 0.0
 integ_runtime = 0.0
-bias = 0.5
+bias = 0.35
+t0 = s.integrator.t
 
 try
     while t < total_time
@@ -84,7 +85,7 @@ try
         PLOT && plot(s, t; zoom=false, front=false)
         
         # Calculate steering inputs based on cosine wave
-        steering = steering_magnitude * cos(2π * steering_freq * t+bias)
+        steering = steering_magnitude * cos(2π * steering_freq * t + bias)
         set_values = -s.set.drum_radius .* s.integrator[sys.winch_force]
         _vsm_interval = 1
         if t > 1.0
@@ -93,8 +94,8 @@ try
         end
 
         # Step simulation
-        steptime = @elapsed (t_new, integ_steptime) = next_step!(s, set_values; dt, vsm_interval=_vsm_interval)
-        t = t_new - 10.0  # Adjust for initial stabilization time
+        steptime = @elapsed (t_new, integ_steptime) = next_step!(s, set_values; dt, vsm_interval=vsm_interval)
+        t = t_new - t0  # Adjust for initial stabilization time
 
         # Track performance after initial transient
         if (t > total_time/2)
@@ -105,6 +106,7 @@ try
 
         # Log state variables
         KiteModels.update_sys_state!(sys_state, s)
+        sys_state.time = t
         log!(logger, sys_state)
     end
 catch e
@@ -129,15 +131,15 @@ sl = lg.syslog
 turn_rates_deg = rad2deg.(hcat(sl.turn_rates...))
 v_reelout_23 = [sl.v_reelout[i][2] for i in eachindex(sl.v_reelout)], [sl.v_reelout[i][3] for i in eachindex(sl.v_reelout)] # Winch 2 and 3
 aero_force_z = [sl.aero_force_b[i][3] for i in eachindex(sl.aero_force_b)]
-aero_moment_y = [sl.aero_moment_b[i][2] for i in eachindex(sl.aero_moment_b)]
+aero_moment_z = [sl.aero_moment_b[i][3] for i in eachindex(sl.aero_moment_b)]
 twist_angles_deg = rad2deg.(hcat(sl.twist_angles...))
 AoA_deg = rad2deg.(sl.AoA)
 heading_deg = rad2deg.(sl.heading)
 
-p = plotx(sl.time .- 10,
+p = plotx(sl.time,
     [turn_rates_deg[1,:], turn_rates_deg[2,:], turn_rates_deg[3,:]],
     v_reelout_23,
-    [aero_force_z, aero_moment_y],
+    [aero_force_z, aero_moment_z],
     [twist_angles_deg[1,:], twist_angles_deg[2,:], twist_angles_deg[3,:], twist_angles_deg[4,:]],
     [AoA_deg],
     [heading_deg];
@@ -146,7 +148,7 @@ p = plotx(sl.time .- 10,
     labels=[
         [L"\omega_x", L"\omega_y", L"\omega_z"],
         ["v_ro[2]", "v_ro[3]"],
-        [L"F_{aero,z}", L"M_{aero,y}"],
+        [L"F_{aero,z}", L"M_{aero,z}"],
         ["twist[1]", "twist[2]", "twist[3]", "twist[4]"],
         ["AoA"],
         ["heading"]
diff --git a/src/mtk_model.jl b/src/mtk_model.jl
index b4689d344..bfaf41e74 100644
--- a/src/mtk_model.jl
+++ b/src/mtk_model.jl
@@ -271,12 +271,13 @@ function force_eqs!(s, system, eqs, defaults, guesses;
         inertia = 1/3 * (s.set.mass/length(groups)) * (norm(group.chord))^2 # plate inertia around leading edge
         @assert !(inertia ≈ 0.0)
         @parameters twist_damp = 50
+        @parameters max_twist = deg2rad(90)
 
         eqs = [
             eqs
             group_tether_moment[group.idx] ~ sum(tether_moment[group.idx, :])
             twist_α[group.idx] ~ (group_aero_moment[group.idx] + group_tether_moment[group.idx]) / inertia
-            twist_angle[group.idx] ~ clamp(free_twist_angle[group.idx], -π/2, π/2)
+            twist_angle[group.idx] ~ clamp(free_twist_angle[group.idx], -max_twist, max_twist)
         ]
         if group.type == DYNAMIC
             eqs = [
@@ -618,6 +619,20 @@ function diff_eqs!(s, eqs, defaults; tether_kite_force, tether_kite_moment, aero
     return eqs, defaults
 end
 
+function calc_R_v_w(kite_pos, e_x)
+    z = sym_normalize(kite_pos)
+    y = sym_normalize(z × e_x)
+    x = y × z
+    return [x y z]
+end
+
+function calc_R_t_w(elevation, azimuth)
+    x = rotate_around_z(rotate_around_y([0, 0, -1], -elevation), azimuth)
+    z = rotate_around_z(rotate_around_y([1, 0, 0], -elevation), azimuth)
+    y = z × x
+    return [x y z]
+end
+
 """
     scalar_eqs!(s, eqs, measure; R_b_w, wind_vec_gnd, va_kite_b, kite_pos, kite_vel, kite_acc, twist_angle, twist_ω)
 
@@ -663,7 +678,7 @@ function scalar_eqs!(s, eqs, measure; R_b_w, wind_vec_gnd, va_kite_b, kite_pos,
         va_kite_b ~ R_b_w' * va_kite
     ]
     @variables begin
-        heading_x(t)
+        heading(t)
         turn_rate(t)[1:3]
         turn_acc(t)[1:3]
         azimuth(t)
@@ -682,9 +697,7 @@ function scalar_eqs!(s, eqs, measure; R_b_w, wind_vec_gnd, va_kite_b, kite_pos,
         simple_twist_angle(t)[1:2]
         simple_twist_ω(t)[1:2]
         R_v_w(t)[1:3, 1:3]
-        view_z(t)[1:3]
-        view_y(t)[1:3]
-        view_x(t)[1:3]
+        R_t_w(t)[1:3, 1:3]
         distance(t)
         distance_vel(t)
         distance_acc(t)
@@ -695,21 +708,18 @@ function scalar_eqs!(s, eqs, measure; R_b_w, wind_vec_gnd, va_kite_b, kite_pos,
     x´´, y´´, z´´ = kite_acc
 
     half_len = length(twist_angle)÷2
+    heading_vec = R_t_w' * R_v_w[:,1]
 
     eqs = [
         eqs
-        view_z          ~ sym_normalize(kite_pos)
-        view_y          ~ view_z × e_y
-        view_x          ~ view_y × view_z
-        R_v_w[:,1]      ~ view_x
-        R_v_w[:,2]      ~ view_y
-        R_v_w[:,3]      ~ view_z
-        heading_x       ~ calc_heading(e_x)
+        vec(R_v_w)     .~ vec(calc_R_v_w(kite_pos, e_x))
+        vec(R_t_w)     .~ vec(calc_R_t_w(elevation, azimuth))
+        heading         ~ atan(heading_vec[2], heading_vec[1])
         turn_rate       ~ R_v_w' * (R_b_w * ω_b) # Project angular velocity onto view frame
         turn_acc        ~ R_v_w' * (R_b_w * α_b)
         distance        ~ norm(kite_pos)
-        distance_vel    ~ kite_vel ⋅ view_z
-        distance_acc    ~ kite_acc ⋅ view_z
+        distance_vel    ~ kite_vel ⋅ R_v_w[:,3]
+        distance_acc    ~ kite_acc ⋅ R_v_w[:,3]
 
         elevation           ~ atan(z / x)
         # elevation_vel = d/dt(atan(z/x)) = (x*ż' - z*ẋ')/(x^2 + z^2) according to wolframalpha
@@ -721,9 +731,9 @@ function scalar_eqs!(s, eqs, measure; R_b_w, wind_vec_gnd, va_kite_b, kite_pos,
         azimuth_vel         ~ (-y*x´ + x*y´) / 
                                 (x^2 + y^2)
         azimuth_acc         ~ ((x^2 + y^2)*(-y*x´´ + x*y´´) + 2(y*x´ - x*y´)*(x*x´ + y*y´))/(x^2 + y^2)^2
+        course              ~ atan(-azimuth_vel, elevation_vel)
         x_acc               ~ kite_acc ⋅ e_x
         y_acc               ~ kite_acc ⋅ e_y
-        course              ~ atan(-azimuth_vel, elevation_vel)
 
         angle_of_attack     ~ calc_angle_of_attack(va_kite_b) + 0.5twist_angle[half_len] + 0.5twist_angle[half_len+1]
         simple_twist_angle[1] ~ sum(twist_angle[1:half_len]) / half_len
@@ -763,14 +773,8 @@ function linear_vsm_eqs!(s, eqs, guesses; aero_force_b, aero_moment_b, group_aer
     @assert length(s.point_system.groups) == length(sol.group_moment_dist)
 
     y_ = [init_va_b; zeros(length(s.point_system.groups)); zeros(3)]
-    jac_, x_ = VortexStepMethod.linearize(
-        s.vsm_solver, 
-        s.aero, 
-        y_;
-        va_idxs=1:3, 
-        omega_idxs=4:6,
-        theta_idxs=7:6+length(s.point_system.groups), 
-        moment_frac=s.point_system.groups[1].moment_frac)
+    x_ = zeros(3+3+length(s.point_system.groups))
+    jac_ = zeros(length(x_), length(y_))
 
     @parameters begin
         last_y[eachindex(y_)] = y_
diff --git a/src/point_mass_system.jl b/src/point_mass_system.jl
index aed09fc85..9dd82b7dd 100644
--- a/src/point_mass_system.jl
+++ b/src/point_mass_system.jl
@@ -235,9 +235,9 @@ function calc_pos(wing::RamAirWing, gamma, frac)
     return pos
 end
 
-function create_tether(idx, set, points, segments, tethers, attach_point, type, dynamics_type)
+function create_tether(idx, set, points, segments, tethers, attach_point, type, dynamics_type, z=[0,0,1])
     segment_idxs = Int16[]
-    winch_pos = find_z_axis_point(attach_point.pos_b, set.l_tether)
+    winch_pos = find_axis_point(attach_point.pos_b, set.l_tether, z)
     dir = winch_pos - attach_point.pos_b
     for i in 1:set.segments
         frac = i / set.segments
@@ -266,40 +266,18 @@ function cad_to_body_frame(wing::RamAirWing, pos)
     return wing.R_cad_body * (pos + wing.T_cad_body)
 end
 
-"""
-    find_z_axis_point(P, l) -> AbstractVector
-
-Find the point on the z-axis that is at a distance `l` from point `P`,
-in the negative z-direction relative to `P[3]`.
-
-Let point `P` be `[x_p, y_p, z_p]`. We are looking for a point `A = [0, 0, z]`
-on the z-axis. The distance between `P` and `A` is `l`.
-
-The calculation is based on the distance formula:
-  `sqrt((x_p - 0)² + (y_p - 0)² + (z_p - z)²) = l`
-  `sqrt(x_p² + y_p² + (z_p - z)²) = l`
-Squaring both sides gives:
-  `x_p² + y_p² + (z_p - z)² = l²`
-Solving for `z`:
-  `(z_p - z)² = l² - x_p² - y_p²`
-  `z_p - z = ±sqrt(l² - x_p² - y_p²)`
-  `z = z_p ± sqrt(l² - x_p² - y_p²)`
-Since we want the point in the negative z-direction relative to `P[3]` (which is `z_p`),
-we choose the most negative solution:
-  `z = z_p - sqrt(l² - x_p² - y_p²)`
-
-# Arguments
-- `P::AbstractVector`: The reference point `[x_p, y_p, z_p]`.
-- `l::Real`: The desired distance from point `P` to the point on the z-axis.
-
-# Returns
-- `AbstractVector`: A 3-element vector `[0, 0, z]` representing the point on the z-axis.
-"""
-function find_z_axis_point(P, l)
-    d = l^2 - P[1]^2 - P[2]^2
-    d < 0 && error("No real solution: l is too small")
-    z = P[3] - sqrt(d)
-    return [0, 0, z]
+# Find the point on the z-axis with distance l from P in the negative direction
+# TODO: rename P to pos
+function find_axis_point(P, l, v=[0,0,1])
+    # Compute dot product v · P
+    v ⋅ P = v[1] * P[1] + v[2] * P[2] + v[3] * P[3]
+    # Compute discriminant
+    D = (v ⋅ P)^2 - norm(v)^2 * (norm(P)^2 - l^2)
+    D < 0 && error("No real solution: l is too small or parameters invalid")
+    # Solve quadratic for t, choose solution for negative direction
+    t = (v ⋅ P - √D) / norm(v)^2
+    # Compute point Q = t * v
+    return [t * v[1], t * v[2], t * v[3]]
 end
 
 function create_ram_point_system(set::Settings, wing::RamAirWing)
@@ -316,6 +294,7 @@ function create_ram_point_system(set::Settings, wing::RamAirWing)
     bridle_top_right = [bridle_top_left[i] .* [1, -1, 1] for i in eachindex(set.top_bridle_points)]
 
     dynamics_type = set.quasi_static ? STATIC : DYNAMIC
+    z = wing.R_cad_body[:,3]
 
     function create_bridle(bridle_top, gammas)
         i_pnt = length(points) # last point idx
@@ -349,13 +328,13 @@ function create_ram_point_system(set::Settings, wing::RamAirWing)
             Point(9+i_pnt, bridle_top[3], dynamics_type)
             Point(10+i_pnt, bridle_top[4], dynamics_type)
 
-            Point(11+i_pnt, bridle_top[2] .+ [0, 0, -1], dynamics_type)
+            Point(11+i_pnt, bridle_top[2] .+ -1z, dynamics_type)
 
-            Point(12+i_pnt, bridle_top[1] .+ [0, 0, -2], dynamics_type)
-            Point(13+i_pnt, bridle_top[3] .+ [0, 0, -2], dynamics_type)
+            Point(12+i_pnt, bridle_top[1] .+ -2z, dynamics_type)
+            Point(13+i_pnt, bridle_top[3] .+ -2z, dynamics_type)
 
-            Point(14+i_pnt, bridle_top[1] .+ [0, 0, -4], dynamics_type)
-            Point(15+i_pnt, bridle_top[3] .+ [0, 0, -4], dynamics_type)
+            Point(14+i_pnt, bridle_top[1] .+ -4z, dynamics_type)
+            Point(15+i_pnt, bridle_top[3] .+ -4z, dynamics_type)
         ]
         segments = [
             segments
@@ -391,12 +370,12 @@ function create_ram_point_system(set::Settings, wing::RamAirWing)
 
     gammas = [-3/4, -1/4, 1/4, 3/4] * wing.gamma_tip
     create_bridle(bridle_top_left, gammas[[1,2]])
-    create_bridle(bridle_top_right, gammas[[4,3]])
+    create_bridle(bridle_top_right, gammas[[3,4]])
 
-    points, segments, tethers, left_power_idx = create_tether(1, set, points, segments, tethers, attach_points[1], POWER, dynamics_type)
-    points, segments, tethers, right_power_idx = create_tether(2, set, points, segments, tethers, attach_points[3], POWER, dynamics_type)
-    points, segments, tethers, left_steering_idx = create_tether(3, set, points, segments, tethers, attach_points[2], STEERING, dynamics_type)
-    points, segments, tethers, right_steering_idx = create_tether(4, set, points, segments, tethers, attach_points[4], STEERING, dynamics_type)
+    points, segments, tethers, left_power_idx = create_tether(1, set, points, segments, tethers, attach_points[1], POWER, dynamics_type, z)
+    points, segments, tethers, right_power_idx = create_tether(2, set, points, segments, tethers, attach_points[3], POWER, dynamics_type, z)
+    points, segments, tethers, left_steering_idx = create_tether(3, set, points, segments, tethers, attach_points[2], STEERING, dynamics_type, z)
+    points, segments, tethers, right_steering_idx = create_tether(4, set, points, segments, tethers, attach_points[4], STEERING, dynamics_type, z)
 
     winches = [winches; Winch(1, TorqueControlledMachine(set), [left_power_idx, right_power_idx], set.l_tether)]
     winches = [winches; Winch(2, TorqueControlledMachine(set), [left_steering_idx], set.l_tether)]
@@ -485,7 +464,7 @@ function init!(system::PointMassSystem, set::Settings, R_b_w, Q_b_w)
     end
 
     for winch in winches
-        winch.tether_length = set.l_tether
+        winch.tether_length ≈ 0.0 && (winch.tether_length = set.l_tether)
         winch.tether_vel = 0.0
         @assert !(winch.tether_length ≈ 0)
     end
@@ -497,17 +476,17 @@ function init!(system::PointMassSystem, set::Settings, R_b_w, Q_b_w)
         @assert group.moment_frac ≈ first_moment_frac "All group.moment_frac must be the same."
     end
 
-    min_z = Inf
+    min_point = fill(Inf, 3)
     for point in points
-        if point.pos_b[3] < min_z
-            min_z = point.pos_b[3]
+        if point.pos_b[3] < min_point[3]
+            min_point .= point.pos_b
         end
     end
     for point in points
-        point.pos_w .= R_b_w * [point.pos_b[1], point.pos_b[2], point.pos_b[3] - min_z]
+        point.pos_w .= R_b_w * (point.pos_b .- min_point)
         point.vel_w .= 0.0
     end
-    kite.pos .= R_b_w * [0.0, 0.0, -min_z]
+    kite.pos .= R_b_w * -min_point
     kite.orient .= Q_b_w
     kite.vel .= 0.0
     kite.angular_vel .= 0.0
@@ -564,7 +543,7 @@ function measure_to_q(measure::Measurement, R_cad_body=I(3))
     z = rotate_around_y(z, -measure.elevation)
     x = rotate_around_z(x, measure.azimuth)
     z = rotate_around_z(z, measure.azimuth)
-    R_b_w = hcat(x, z × x, z)
+    R_b_w = R_cad_body' * hcat(x, z × x, z)
     Q_b_w = rotation_matrix_to_quaternion(R_b_w)
     return Q_b_w, R_b_w
 end
diff --git a/src/ram_air_kite.jl b/src/ram_air_kite.jl
index 058ed48cc..06b1a2a52 100644
--- a/src/ram_air_kite.jl
+++ b/src/ram_air_kite.jl
@@ -132,7 +132,7 @@ function update_sys_state!(ss::SysState, s::RamAirKite, zoom=1.0)
     ss.time = s.integrator.t # Use integrator time
 
     # Get the extended state vector from the integrator
-    set_values, pos, acc_vec, Q_b_w, elevation, azimuth, course, heading_x, tether_length, tether_vel, winch_force,
+    set_values, pos, acc_vec, Q_b_w, elevation, azimuth, course, heading, tether_length, tether_vel, winch_force,
         twist_angle, kite_vel, aero_force_b, aero_moment_b, turn_rate, va_kite_b, wind_vec_gnd, wind_vel_kite = s.get_state(s.integrator)
 
     P = length(s.point_system.points)
@@ -160,7 +160,7 @@ function update_sys_state!(ss::SysState, s::RamAirKite, zoom=1.0)
     ss.twist_angles[1:num_groups] .= twist_angle
     ss.depower = rad2deg(mean(twist_angle)) # Average twist for depower
     ss.steering = rad2deg(twist_angle[end] - twist_angle[1])
-    ss.heading = heading_x # Use heading_x from MTK model
+    ss.heading = heading # Use heading from MTK model
     ss.course = course
     # Apparent Wind and Aerodynamics
     ss.v_app = norm(va_kite_b)
@@ -391,7 +391,7 @@ function generate_getters!(s, sym_vec)
          sys.elevation,          # Elevation angle
          sys.azimuth,            # Azimuth angle
          sys.course,             # Course angle
-         sys.heading_x,          # Heading angle (based on body x-axis projection)
+         sys.heading,          # Heading angle (based on body x-axis projection)
          c(sys.tether_length),   # Unstretched length per winch
          c(sys.tether_vel),      # Reeling velocity per winch
          c(sys.winch_force),     # Force at winch connection point per winch
diff --git a/test/test_ram_air_kite.jl b/test/test_ram_air_kite.jl
index 622c1440c..ae1e2d5ec 100644
--- a/test/test_ram_air_kite.jl
+++ b/test/test_ram_air_kite.jl
@@ -62,9 +62,9 @@ const BUILD_SYS = true
         first_integrator_ptr = objectid(s.integrator)
         first_point_system_ptr = objectid(s.point_system)
 
-        # 2. First reinit! - should load from serialized file
-        @info "Testing first reinit! (should load serialized file)..."
-        @time KiteModels.reinit!(s, measure; prn=true, reload=false)
+        # 2. First init_sim! - should load from serialized file
+        @info "Testing first init_sim! (should load serialized file)..."
+        @time KiteModels.init_sim!(s, measure; prn=true, reload=false)
         next_step!(s)
 
         # Check that it's a new integrator
@@ -73,9 +73,9 @@ const BUILD_SYS = true
         @test first_integrator_ptr != second_integrator_ptr
         @test first_point_system_ptr == second_point_system_ptr
 
-        # 3. Second reinit! - should reuse existing integrator
-        @info "Testing second reinit! (should reuse integrator)..."
-        @time KiteModels.reinit!(s, measure; prn=true, reload=false)
+        # 3. Second init_sim! - should reuse existing integrator
+        @info "Testing second init_sim! (should reuse integrator)..."
+        @time KiteModels.init_sim!(s, measure; prn=true, reload=false)
 
         # This should create a new point system but reuse the existing integrator
         third_integrator_ptr = objectid(s.integrator)
@@ -108,7 +108,7 @@ const BUILD_SYS = true
     end
 
     @testset "State Consistency" begin
-        KiteModels.reinit!(s, measure, prn=true, reload=false)
+        KiteModels.init_sim!(s, measure, prn=true, reload=false)
         sys_state_before = KiteModels.SysState(s)
 
         # Check quaternion normalization
@@ -120,7 +120,7 @@ const BUILD_SYS = true
         # Change measurement and reinitialize
         old_elevation = measure.elevation
         measure.sphere_pos[1,:] .= deg2rad(85.0)
-        KiteModels.reinit!(s, measure, prn=true, reload=false)
+        KiteModels.init_sim!(s, measure, prn=true, reload=false)
 
         # Get new state using SysState
         sys_state_after = KiteModels.SysState(s)
@@ -132,22 +132,24 @@ const BUILD_SYS = true
 
     function test_step(s, d_set_values=zeros(3); dt=0.05, steps=5)
         s.integrator.ps[s.sys.stabilize] = true
-        KiteModels.next_step!(s; dt=10.0)
+        for i in 1:1÷dt
+            next_step!(s; dt, vsm_interval=1)
+        end
         s.integrator.ps[s.sys.stabilize] = false
         @info "Stepping"
         for _ in 1:steps
             set_values = -s.set.drum_radius * s.integrator[s.sys.winch_force] + d_set_values
             KiteModels.next_step!(s, set_values; dt)
-            # Use SysState to get heading_x if needed, or directly from integrator if simpler
+            # Use SysState to get heading if needed, or directly from integrator if simpler
             # sys_state_step = KiteModels.SysState(s)
-            # @show sys_state_step.heading_x # Example if heading_x is in SysState
-            @show s.integrator[s.sys.heading_x] # Keep direct access if simpler for this specific value
+            # @show sys_state_step.heading # Example if heading is in SysState
+            @show s.integrator[s.sys.heading] # Keep direct access if simpler for this specific value
         end
     end
 
     @testset "Simulation Step with SysState" begin
         # Basic step and time advancement test
-        KiteModels.reinit!(s, measure; prn=true, reload=false)
+        KiteModels.init_sim!(s, measure; prn=true, reload=false)
         sys_state_before = KiteModels.SysState(s)
 
         # Run a simulation step with zero set values
@@ -175,7 +177,7 @@ const BUILD_SYS = true
             @test measure.elevation ≈ deg2rad(60.0) atol=1e-6
             @test measure.azimuth ≈ 0.0 atol=1e-6
 
-            KiteModels.reinit!(s, measure; prn=true)
+            KiteModels.init_sim!(s, measure; prn=true)
 
             # Verify initial conditions using SysState
             sys_state_init = KiteModels.SysState(s)
@@ -206,17 +208,17 @@ const BUILD_SYS = true
         @testset "Steering Response Using SysState" begin
             # Initialize model at moderate elevation
             measure.sphere_pos .= deg2rad.([70.0 70.0; 1.0 -1.0])
-            KiteModels.reinit!(s, measure; prn=true, reload=false)
+            KiteModels.init_sim!(s, measure; prn=true, reload=false)
             test_step(s)
             sys_state_initial = KiteModels.SysState(s)
 
             # steering right
-            KiteModels.reinit!(s, measure; prn=true, reload=false)
+            KiteModels.init_sim!(s, measure; prn=true, reload=false)
             test_step(s, [0, 10, -10]; steps=20)
             sys_state_right = KiteModels.SysState(s)
 
             # steering left
-            KiteModels.reinit!(s, measure; prn=true, reload=false)
+            KiteModels.init_sim!(s, measure; prn=true, reload=false)
             test_step(s, [0, -10, 10]; steps=20)
             sys_state_left = KiteModels.SysState(s)
 
@@ -227,9 +229,9 @@ const BUILD_SYS = true
 
             # Check heading changes
             right_heading_diff = angle_diff(sys_state_right.heading, sys_state_initial.heading)
-            @test right_heading_diff ≈ 0.6 atol=0.2
+            @test right_heading_diff ≈ 0.9 atol=0.2
             left_heading_diff = angle_diff(sys_state_left.heading, sys_state_initial.heading)
-            @test left_heading_diff ≈ -0.6 atol=0.2
+            @test left_heading_diff ≈ -0.9 atol=0.2
         end
     end
 end