Robotics Math

With zero rotation, the transform is just a translation: add the robot position to the point. (1,0) in robot frame becomes (4,4) in world frame.

The robot faces 90° (along the y-axis). A point 1 unit "ahead" in robot frame (robot's x-axis) is actually 1 unit along the world's y-axis. So world position = (2, 1+1) = (2, 2).

x_world = 2. The chain works inside out: point (1,0) in tool frame becomes (2,1) in base frame (the tool frame is rotated 90° and offset by 2), then (2,2) in world frame (base is offset by 1 in y).

The inverse of a homogeneous transform is NOT just the transpose (that only works for pure rotation matrices). The inverse is T^-1 = [[R^T, -R^Tt], [0, 1]]. You rotate the translation vector by R^T and negate it. This is computationally cheap — no general matrix inversion needed.

The pipeline reads left to right but the math reads right to left: p_world = T_{world_car} · T_{car_lidar} · p_lidar. Start with the point, apply the LiDAR-to-car transform, then the car-to-world transform.

The x-axis unit vector rotates to the y-axis unit vector. A 90° yaw maps "forward" to "left" (in a right-handed frame). The z-component stays 0 because R_z doesn't affect z.

At β = 90°, cosβ = 0. Plug into R_z(γ)R_y(90°)R_x(α) and you get a matrix that only depends on γ+α, not on γ and α independently. Two parameters collapsed into one — you lost a degree of freedom. This is gimbal lock. The fix: use quaternions, which have no singular configurations.

Notice the half-angle: 90° rotation uses 45° in the quaternion. This is because quaternions double-cover SO(3) — both q and -q represent the same rotation. The norm is ||q|| = √(0.5 + 0 + 0 + 0.5) = 1, confirming it's a valid unit quaternion.

z = 0 because the rotation axis is [1,0,0], which has no z component. A 180° rotation has w=0 (cos(90°)=0), meaning the quaternion is purely imaginary. This represents the "most different" orientation from identity (where q = [1,0,0,0]).

Yes, it is a valid unit quaternion. It represents a 120° rotation about the axis [1,1,1]/√3. You can verify: w = cos(θ/2) = 0.5 implies θ/2 = 60°, so θ = 120°. The axis is [0.5, 0.5, 0.5]/sin(60°) = [0.5, 0.5, 0.5]/0.866 = [0.577, 0.577, 0.577] ≈ [1,1,1]/√3.

Facing east (θ=0), cos(0)=1, sin(0)=0. The robot drives 1 meter along the x-axis. Simple — but the math gets interesting when θ ≠ 0.

At 45°, the robot moves equally in x and y. It travels a distance of v×dt = 1 meter, split as (cos45°, sin45°) = (0.707, 0.707).

This entry means: "if heading changes by a small δθ, the x-position prediction changes by -0.5 × δθ." When facing north (θ=π/2), a small increase in heading rotates the velocity vector slightly to the left, reducing x — hence the negative sign.

P^-(1,1) = 1 + 0.01 = 1.01. The x uncertainty barely grew because at θ=0, x-position doesn't depend on θ (F(1,3)=0). But look at P^-(2,2) = 1.11: the y uncertainty grew more because F(2,3)=1, meaning y-prediction couples with θ-uncertainty.

Line 3 is the bug. The y update uses Math.cos(theta) instead of Math.sin(theta). When θ=π/2 (facing north), cos(π/2)=0 so y doesn't change — the robot "drifts right" because x increases (cos is nonzero at other angles) while y stays stuck. The correct line: const yNew = y + v * Math.sin(theta) * dt;

The motion model is x' = x + Δx, y' = y + Δy, θ' = θ + Δθ. Taking ∂x'/∂x = 1 because x appears directly in x'. The Jacobian has the form F = I + (off-diagonal terms from the nonlinear corrections). This is the hallmark of an additive motion model.

The classic 3-4-5 right triangle. The predicted range is simply the Euclidean distance from robot to landmark.

The landmark is at 53.13° from the robot's heading direction. Since the robot faces east (θ=0), this is the same as the absolute angle to the landmark.

This is -cos(φ) where φ is the bearing angle. The negative sign means: if the robot moves in +x direction, the range to the landmark decreases (because it's moving toward the landmark's x-component). The magnitude 0.6 = cos(53.13°) reflects that the landmark isn't directly ahead.

The measured range is 0.2m shorter than predicted. This means the landmark is closer than we thought, so the update step will pull the robot estimate toward the landmark. The innovation is the "surprise" — what the sensor told us that we didn't already know.

javascript
function ekfUpdate(x, P, z, R) {
  const K = P / (P + R);      // Kalman gain
  const xNew = x + K * (z - x); // updated state
  const PNew = (1 - K) * P;    // updated covariance
  return [xNew, PNew];
}

K = P/(P+R). As R → 0, K → P/P = 1. The updated state becomes x⁺ = x^- + 1 × (z - x^-) = z. A perfect sensor overrides the prediction entirely. Conversely, as R → ∞, K → 0 and the measurement is ignored. The Kalman gain is exactly the trust slider between prediction and measurement.

In 3D SLAM, each pose has 6 DOF (x, y, z, roll, pitch, yaw), so 100 poses = 600 variables. If landmarks are also optimized (as in full SLAM), add 2 variables per 2D landmark or 3 per 3D landmark.

The error is negative: the actual displacement was 0.1m more than the odometry predicted. The optimizer will try to reduce this error by adjusting x₁ and/or x₂. Weighted by Ω, larger errors in more-certain measurements get penalized more heavily.

Higher information = more certainty. The x-position has Ω=100, while θ has Ω=1/0.001=1000. The angular measurement is 10x more informative, so angular errors are penalized 10x more heavily in the optimization.

The graph optimizer adjusts ALL poses simultaneously. A loop closure reveals that drift has accumulated over 50 steps. Rather than slamming x₅₀ onto x₁ (which would violate all the odometry constraints), the optimizer spreads the correction across all intermediate poses. Each pose shifts a little, and the total cost is minimized. This is the power of graph-based SLAM over filtering.

That's 56 million entries. But H is extremely sparse — each edge only connects two nodes, so each row has at most 6+6=12 nonzero entries. Sparse linear solvers like CHOLMOD exploit this sparsity to solve in milliseconds, not hours.

Both links point along the x-axis, so the end effector is at (2, 0). This is the maximum reach: L₁ + L₂ = 2.

The first link points at 30°, and the second link at 30°+45°=75° (angles are cumulative). The end effector is at (1.125, 1.466), about 1.85m from the origin — less than the max reach of 2m because the arm is bent.

The elbow bends at exactly 90° to reach (1, 1). This makes geometric sense: the target is at distance √2 from the origin, and two unit-length links forming a right angle span exactly √(1²+1²) = √2.

(3, 1) is at distance √(9+1) = √10 ≈ 3.16, which exceeds the maximum reach L₁+L₂ = 3. Even fully extended, the arm can only reach 3 meters. The workspace is an annulus (donut) with inner radius |L₁-L₂|=1 and outer radius L₁+L₂=3.

javascript
function forwardKinematics2Link(L1, L2, theta1, theta2) {
  const x = L1 * Math.cos(theta1) + L2 * Math.cos(theta1 + theta2);
  const y = L1 * Math.sin(theta1) + L2 * Math.sin(theta1 + theta2);
  return [x, y];
}

Rotating joint 1 moves the end effector in the (-x, +y) direction. Rotating joint 2 moves it in the -x direction only. The determinant is (-1)(0) - (-1)(1) = 1 ≠ 0, so we're not at a singularity.

Joint 1 rotating at 1 rad/s swings the end effector at (-1, 1) m/s. The speed is √(1+1) = √2 ≈ 1.41 m/s. The end effector moves along a circle, and the Jacobian gives the instantaneous linear velocity of that circular motion.

Zero determinant means the Jacobian is singular — it cannot be inverted. Physically, the fully extended arm can only move perpendicular to itself (sideways). It cannot move radially (extend or contract) because both joints produce the same motion direction. This is the kinematic equivalent of two equations being linearly dependent.

Maximum manipulability at θ₂=90° means the arm can move equally well in all directions. This is the "sweet spot" for control — the arm is most agile with a right-angle elbow bend. Robot path planners try to keep joints away from singularities (θ₂=0 or 180°) to maintain good manipulability.

Line 7 is the bug. J(2,2) = ∂y/∂θ₂ = L₂·cos(θ₁+θ₂) = L2*c12, but the code uses L2*s12 (sine instead of cosine). This would give wrong end-effector velocities whenever joint 2 is moving. The correct line: const J22 = L2*c12;

Pure proportional control: the output is simply the gain times the error. As the system approaches the target, the error shrinks and the correction weakens. This is why P-only control has steady-state error — the correction vanishes before reaching zero error.

The P term says "error is 1, push with force 1." The I term says "errors have been accumulating (total 9), push a little extra." The D term says "error is decreasing (from 3 to 1), back off to avoid overshoot." The net command 0.9 is a gentle push — the system is converging nicely.

Rapid oscillation around the target is the hallmark of excessive K_p. The controller overcorrects: it pushes too hard, overshoots, sees a negative error, pushes back too hard, overshoots again. K_i issues cause slow drift/overshoot (not rapid oscillation). K_d issues cause jitter from noise, not sustained oscillation. Fix: reduce K_p and/or add K_d for damping.

javascript
function pidStep(e, ePrev, integral, Kp, Ki, Kd, dt) {
  const newIntegral = integral + e * dt;
  const P = Kp * e;
  const I = Ki * newIntegral;
  const D = Kd * (e - ePrev) / dt;
  return [P + I + D, newIntegral];
}

Steady-state error occurs when the proportional correction exactly balances the disturbance (gravity). The system is in equilibrium: K_p×e = gravity force. The error e is nonzero because that's what generates the force to hold position. Adding K_i breaks this equilibrium: the integral accumulates over time, continuously increasing the control effort until e=0. Only then does the integral stop growing. This is why the I term exists — it's the only PID component that guarantees zero steady-state error.

The D term is negative because the error is decreasing rapidly. It acts as a brake — opposing the direction of movement to prevent overshoot. The magnitude is large (-30) because the error changed by 3 in just 0.1 seconds, so the derivative (rate of change) is -30/s. This is why K_d must be tuned carefully: too much and the system sluggishly approaches the target.

The EKF loop: (1) Predict the new pose using the motion model, (2) Detect a landmark via LiDAR, (3) Innovate — compare predicted vs actual measurement, (4) Update the state estimate using the Kalman gain, (5) Control — compute the next motor command based on the updated pose estimate.

New state: [5.866, 3.5, 0.624]. The robot moved mostly in x (because cos30° > sin30°) and turned slightly leftward (heading increased by 0.1 rad ≈ 5.7°).

The landmark is about 5.4 meters away. If the LiDAR measures 5.3m instead, the innovation is -0.12m — suggesting the robot is slightly closer to the landmark than predicted. The EKF update will pull the pose estimate toward the landmark.

K = 0.71, meaning we trust the measurement about 71% (because R=0.2 is smaller than P=0.5). The updated position shifts 71% of the way from the prediction toward the measurement. The updated P⁺ = (1-0.71) × 0.5 = 0.143, much smaller than both P and R — fusion always improves certainty.

The covariance dropped from 0.5 to 0.143 — the uncertainty decreased by 71%. This is the fundamental guarantee of the Kalman filter: fusing two uncertain estimates always produces a better estimate than either one alone. The updated covariance is always smaller than both the prediction covariance P and the measurement noise R.

The P controller commands 2.1 m/s. In practice, you'd clamp this to the robot's maximum speed. As the robot approaches the waypoint, the error shrinks and the velocity naturally decreases — this is the beauty of proportional control for waypoint following. A real system would add K_d to avoid overshoot at the waypoint.

F(2,3)=0.866 says: a small change in heading increases the y-velocity component. At θ=30°, the robot is mostly moving in x; a small heading increase rotates velocity toward y. F(1,3)=-0.5 = -sin(30°) says: that same heading change reduces x-velocity, because rotating away from the x-axis.