Conversation
Working on developing ekf listeners and ekf processing code
|
|
||
| def state_to_measurement_h(self, x, u): | ||
| #currently 3 element point | ||
| z = np.array([x[13],x[14],x[15]]) # from substate + 3 points |
There was a problem hiding this comment.
Here is where you could calculate what the relative position of the object should be, given some estimated positio of the sub and object from the x vector
There was a problem hiding this comment.
not sure if this is ok, i currently have the state_to_measurement_h just returning the same camera position of the object, and the x will just contain the raw camera position of each object. I'm doing the processing of camera position to absolute position in the construction of the world position message. I'm trying to get it so that x has relative object positions instead of raw camera positions, but if using the raw camera positions is fine, then i'm just going to leav eit.
There was a problem hiding this comment.
I think making x have global positions of objects is the best bet. If x stores the relative location, we would need to update it for each object whenever the sub moves, and I think that would add too much noise.
Something like:
x: global positions of all objects
incoming message: has relative location of object as seen by camera
z function: returns the relative measurement
state_to_measurement_h: calculates what the relative measurement should be given the two absolution positions and sub orientation
There was a problem hiding this comment.
While I was working on that, I was considering how to deal with the variance of the object positions, cuz they're still coming out of the get_R function as relative. They're supposed to be kinda small though, so would it be ok if they just stayed relative until the data published as a message? Otherwise, I could try changing the get_R function to also take x and u so i can calculate the absolute values.
There was a problem hiding this comment.
The get_R function should be returning the uncertainty of the measurements (relative position), not the state (absolute position).
…ne/aquadrone2020 into ekf_positioning
…ne/aquadrone2020 into ekf_positioning
| message.z = vec[2] | ||
| return message | ||
|
|
||
| def quat_to_euler(q): |
There was a problem hiding this comment.
Checkout scipy.spatial.transform.Rotation
…ne/aquadrone2020 into world_positioning
3 participants
This update is for world positioning, currently only set up for a single object as a proof of concept before moving forward