Update testLIEKF.cpp
							parent
							
								
									a0c6902c8e
								
							
						
					
					
						commit
						4f2a62aa3a
					
				|  | @ -26,14 +26,14 @@ namespace example { | |||
| static constexpr double k = 0.5; | ||||
| Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) { | ||||
|   // φ = Logmap(R), Dφ = ∂φ/∂δR
 | ||||
|   Matrix3 Dφ; | ||||
|   Vector3 φ = Rot3::Logmap(X, Dφ); | ||||
|   Matrix3 D_phi; | ||||
|   Vector3 phi = Rot3::Logmap(X, D_phi); | ||||
|   // zero out yaw
 | ||||
|   φ[2] = 0.0; | ||||
|   Dφ.row(2).setZero(); | ||||
|   phi[2] = 0.0; | ||||
|   D_phi.row(2).setZero(); | ||||
| 
 | ||||
|   if (H) *H = -k * Dφ;  // ∂(–kφ)/∂δR
 | ||||
|   return -k * φ;        // xi ∈ 𝔰𝔬(3)
 | ||||
|   if (H) *H = -k * D_phi;  // ∂(–kφ)/∂δR
 | ||||
|   return -k * phi;         // xi ∈ 𝔰𝔬(3)
 | ||||
| } | ||||
| }  // namespace example
 | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue