2009 AIAA Guidance, Navigation and Control Conference Real-Time Mosaic-Aided Aerial Navigation: II. Sensor Fusion ����� �������������� ������ ������������������������������� ������������������������������� ����� �!��� "������������������������� #������ ������� �$��� August 2009
����������� … ��������� Camera scanning � Introduction On-line mosaic construction � Image-based motion estimation � � Mosaicking improves estimation precision in challenging scenarios � Narrow camera FOV � Low-texture scene �������������������������� � Pos ���� � ��������� ������������ V � Ψ ��������� ��������� Now Now ������� ������ ������� Part I Part I ������������ ������ ������ ������ ����� ������� ���������� ����������������������� �
�������� � Introduction � Relative Motion Measurement Model � Fusion with Navigation System � Observability Analysis � Performance Evaluation �������������������������� � Pos � Summary ���� � ��������� V ������������ � Ψ ��������� ��������� Now Now ������� ������ ������� ������������ ������ ������ ������ ����� ������� ���������� �����������������������
���������������������������������� Coordinate systems � Introduction � L - Local Level Local North (LLLN) � B - Body Measurements Model � C - Camera Fusion with Navigation sys. Image-based motion estimation � γ � Observability C - translation (known up to some scale ) t → Analysis 2 � 1 2 C - rotation R 2 � Performance C 1 Evaluation Summary In ideal ideal conditions, when there are no navigation errors and � assuming perfect translation and rotation motion estimations: ���� ���� � L − = γ 2 C C Pos t ( ) Pos t ( ) T t 2 2 → 2 1 L 1 2 2 = C C T R 2 2 C C 1 1 ���� - Platform position Pos � N - DCM from system N to system M T � M !
������������������������������������������ ���� ���� � � L − × = 2 C C Pos t ( ) Pos t ( ) T t 0 2 2 Introduction → 2 1 L 1 2 2 ( ) Measurements T = C C T R I 2 2 Model C C 1 1 Fusion with Navigation sys. In real conditions these constraints do not hold, due to � Observability � Navigation errors Analysis � Imperfect image-based motion estimations Performance Evaluation Summary Residual measurements definition: � ���� ���� L � 2 − × = ˆ C C Pos ( ) t Pos ( ) t T t z 2 2 Nav Nav → 2 1 L N , av 1 2 translation 2 � T = − ˆ C C T R I z 2 2 C Na , v C rota t on i × 1 1 "
������������������������������������������ State vector definition � Introduction � � � � � � T = � � �Ψ ∈ℜ × T T T T T 15 1 X P V d b Measurements Model Continuous system matrix � Fusion with Navigation sys. 0 I 0 0 0 × × × × × 3 3 3 3 3 3 3 3 3 3 Observability B 0 0 A 0 T Analysis × × × 3 3 3 3 s 3 3 L Φ = − ∈ℜ × B 15 15 0 0 0 T 0 × × × × c 3 3 3 3 3 3 L 3 3 Performance 0 0 0 0 0 Evaluation × × × × × 3 3 3 3 3 3 3 3 3 3 0 0 0 0 0 × × × × × 3 3 3 3 3 3 3 3 3 3 Summary - a skew-matrix constructed based on accelerometer A � s sensors readings B - DCM from Body to Local Level Local North systems T � L #
������������������������������������������ Measurements Equations � � � P Introduction � � V � � Measurements Tr Tr Tr Tr z � 0 H H H H �Ψ = × � �Ψ + Model Translation 3 3 V d b v � � Rot Rot z 0 0 H H 0 × × �Ψ × Rotation Fusion with 3 3 3 3 d 3 3 d Navigation sys. � b Observability � = − Analysis t t t 2 1 Performance Translation terms Rotation terms Evaluation Summary = − � Tr C ˆ C L H T t T t 2 2 1 � → V L 1 2 L × 2 2 1 ( )( ) ( ) = − � 2 Tr C ˆ C L H T t T A t t �Ψ = − ˆ 2 2 1 Rot C B L E L H R T T T T I �Ψ → 2 2 2 1 L 1 2 L s 1 × 2 2 2 C C B L E 1 2 2 2 = � ˆ 1 ( ) ( ) Rot C B L B H R T T T t 2 2 2 1 = � 3 Tr C ˆ C L B H T t T A t T t 2 2 1 1 d C C B L → 1 2 2 1 d L 1 2 L s 1 L × 6 2 2 1 1 ( ) = − � 2 Tr C ˆ C L B H T t T T t 2 2 1 1 → b L 1 2 L L × 2 2 2 1 $
����������������������������������� ������� Remarks � Introduction � Motion parameters may be estimated based on the homography or the fundamental matrices Measurements Model Fusion with Navigation sys. � Pos Observability � Analysis V � Ψ Performance Evaluation Summary � ˆ P � ˆ V �Ψ ˆ ˆ d ˆ b %
���������������������� Adaptive translation measurement covariance � Introduction � ( ) ( ) = − � � = − Tr L L ˆ L ˆ L ˆ L L v Pos t Pos t t , t t t 2 2 2 2 2 2 → → → → Nav 2 Nav 1 1 2 1 2 1 2 1 2 × Measurements Model ( ) ( ) ( ) ( ) = − − − Tr L L L L R Pos t Pos t R Pos t Pos t 2 2 2 2 Nav 2 Nav 1 Est Nav 2 Nav 1 × × Fusion with Navigation sys. Measurement covariance matrix � Observability Analysis Performance Tr R 0 = k Evaluation R k Rot 0 R Summary Measurements-rejection mechanism is used to avoid fusion of � low-quality measurements &
����������������������� ������� Fictitious Velocity (FV) measurement � � � Unobservable states in are deteriorated due to X Introduction � ( ) imperfectness in image-based motion estimation C C t 2 , R 2 2 → 1 C Measurements 1 Model � Fictitious Velocity measurement is introduced Fusion with Navigation sys. � Goal – to let the filter “believe” the error along the flight heading is small Observability Analysis � Implementation: � � ( ) T � = L Performance V V 0 Evaluation � ( ) T = FV L H 0 V 0 0 0 Summary × × × × 1 3 1 3 1 3 1 3 Trans H R 0 × = = Rot 6 6 H H R Aug Aug FV 0 R × 1 6 FV H � After the KF gain matrix is computed, the FV data is removed '(
������������� �������� Piece-Wise Constant System (PWCS) [Goshen-Meskin & Bar-Itzhack 1992] � � � Introduction ( ) ( ) ( ) + = + x k 1 F x k B u k j j � � Measurements ( ) ( ) = z k H x k Model j j Fusion with � For each time segment j=1,…,r the system matrices are constant Navigation sys. � At least n measurements in each segment Observability � Observability matrix in each segment Analysis ( ) ( ) T T = H F − T T T T n 1 � Q H H F Performance j j j j j j Evaluation Summary � Total Observability Matrix (TOM) Q 1 − n 1 Q F ( ) = 2 1 Q r � − − − n 1 n 1 � n 1 Q F F F − − r r 1 r 2 1 ''
Recommend
More recommend