The figure below is a representation of a 2D double pendulum. The longer arm of the pendulum is connected to a fixed support and a joint. The shorter arm is connected to a joint on the longer arm and is free swinging on the other end. The shorter arm additionally has two point markers spaced 1.25m from the joint.

The trajectories of the two dots are assumed to be tracked in a motion capture lab with the coordinates recorded each second.

The trajectories are given in sets **m1** and **m2** for the respective points. The trajectory location contains:

- Time
- X-coordinate
- Y-coordinate

## Setting up the optimization problem

We first start out by setting up the optimization problem. The behavior of the system can be fully described by the two angles as the Degrees of Freedom (DOF) are calculated as:

Description | Effect on DOF |
---|---|

Two bodies with x 3 DOF | +6 |

Two revolute joints x 2 DOF | -4 |

Total DOF | 2 |

Giving us two degrees of freedom as 𝜃1 and 𝜃2.

Now we could easily solve this problem for the angles but that wouldn't be an optimization problem. These types of problem are useful when there is measurement error as the solution will converge to the most likely solution within the defined parameters.

In order to do this, we begin the optimization problem by setting up the system of constraints:

The constraints here relate the angles to the x and y coordinates of the points m1 and m2.

In order to evaluate the system state, an obective function must be defined. For this situation we use the least square method which can be written as:

Or, in an easier to read (but improper) form:

We will be passing this obective function to the matlab function 'fminunc' in order to evaluate the system state for each set of points.

## Solving the Optimization Problem in Matlab using fminunc

The code used in matlab to solve for the system state with a set of inputs is as such:

```
%clear all the previous output and variables
clear all
clc
close all
%set the lengths
l1=0.5
l2=0.125
%test data input of the x and y coordinates of m1 and m2
m1x=2.011065770951412e-001
m2x=2.728036316390219e-001
m1y=-5.853569186806583e-001
m2y=-6.877509242167823e-001
%definition of the obective function (ugly because of how fminunc wants the input)
psi=@(theta)1/2*[
l1*cos(theta(1))+l2*cos(theta(2))-m1x;
l1*cos(theta(1))+2*l2*cos(theta(2))-m2x;
l1*sin(theta(1))+l2*sin(theta(2))-m1y;
l1*sin(theta(1))+2*l2*sin(theta(2))-m2y].'*[l1*cos(theta(1))+l2*cos(theta(2))-m1x;
l1*cos(theta(1))+2*l2*cos(theta(2))-m2x;
l1*sin(theta(1))+l2*sin(theta(2))-m1y;
l1*sin(theta(1))+2*l2*sin(theta(2))-m2y]
%solve for the minimum with search start at [2,2]
fminunc(psi,[2,2])
```