I could use some guidance regarding a coordinate system transform problem. My situation is this: my system begins at some unknown location, which I initialize the location (x y) and orientation (roll, pitch, and yaw) all to zero. I establish a frame of reference at this point, which I call the "local" coordinate frame. It is fixed in the world and does not move. At system startup, the body frame is perfectly aligned with the local frame, where body +x points forward, +y to the right and +z down. The body frame is fixed to my system, and travels with the system as it moves.
I have an estimation routine that provides me with the x and y position, as well as the roll, pitch, and yaw of the system. Yaw is rotation about the z axis of the local frame. Pitch and roll are with respect to the body frame (I.e.,if the robot pitches up, I always get a positive value. If it rolls right, I get a positive value.)
How can I take the known roll and pitch values and transform them to be with respect to the local (fixed) frame?