Robust state estimation solutions must deal with faulty measurements, called outliers, and unknown data associations, which lead to multiple feasible hypotheses. Take, for instance, the scenario of tracking two indistinguishable targets based on position measurements, where each measurement could refer to either of the targets or even be a faulty reading. Common estimation methods model the state as having a unimodal distribution, so they are called unimodal methods. Likewise, multimodal methods model the state as a multimodal distribution. Difficult problems, such as autonomous underwater vehicle (AUV) navigation relying on acoustic localization, frequently involve recurring outliers. In these situations, the correct hypothesis only emerges as the most likely one when a substantial number of measurements are considered. Robust solutions for these problems need to consider multiple hypotheses simultaneously, which, in turn, calls for the representation of multimodal distributions. In this work, a novel approximate inference method is presented, called the Gaussian mixture sum-product algorithm (GM-SPA), as it implements the sum-product algorithm (SPA) for Gaussian mixtures. The GM-SPA can exactly represent under-constrained linear measurements and approximate important non-linear models, such as range measurements and 2D pose kinematics. The outlier robustness of the GM-SPA is tested and compared against the particle filter (PF) and multimodal incremental smoothing and mapping (MMiSAM), both of which are non-parametric methods. Robustness, accuracy, and run-time are improved in simulation tests. The test problems include 1D localization with unknown data association, 3D linear target tracking with correlated outliers, and 2D range-only pose estimation with Gaussian mixture noise. / Robusta lösningar för tillståndsuppskattning måste kunna hantera felaktiga mätningar, så kallade outliers, och okända dataassociationer, vilket leder till flera möjliga hypoteser. Ta till exempel scenariot att spåra två likadana mål baserat på positionsmätningar, där varje mätning kan tillhöra något av målen eller till och med vara en felaktig avläsning. Vanliga skattningsmetoder modellerar tillståndet som en unimodal fördelning, och kallas därför unimodala metoder. På samma sätt modellerar multimodala metoder tillståndet som en multimodal fördelning. Svåra problem, som navigering av autonoma undervattensfarkoster (AUV) med hjälp av akustisk lokalisering, involverar ofta upprepade outliers. I dessa situationer framstår den korrekta hypotesen som den mest sannolika först när ett stort antal mätningar beaktas. Robusta lösningar för dessa problem måste ta hänsyn till flera hypoteser samtidigt, vilket i sin tur kräver representation av multimodala fördelningar. I detta arbete presenteras en ny approximativ inferensmetod, kallad Gaussian mixture sum-product algorithm (GM-SPA), eftersom den implementerar sum-product algorithm (SPA) för gaussiska blandningar. GM-SPA kan representera underbegränsade linjära mätningar exakt och approximera viktiga icke-linjära modeller, till exempel avståndsmätningar eller 2D-posekinematik. GM-SPA:s robusthet mot outliers testas och jämförs med partikelfiltret (PF) och multimodal incremental smoothing and mapping (MM-iSAM), som båda är icke-parametriska metoder. Robusthet, noggrannhet och körtid förbättras i simuleringstester. Simulerade tester inkluderar 1D-lokalisering med okänd dataassociation, 3D linjär målföljning med korrelerade outliers och 2D-ställningsuppskattning av endast räckvidd med Gaussiskt blandningsljud. / Soluções robustas para estimação de estado devem lidar com medidas defeituosas, chamadas de outliers, e com associações de dados desconhecidas, que levam a múltiplas hipóteses possíveis. Considere-se, por exemplo, o cenário de rastreamento de dois alvos indistinguíveis com base em medidas de posição, em que cada medida pode-se referir a qualquer um dos alvos ou até mesmo ser uma leitura defeituosa. Métodos de estimação comuns modelam o estado como tendo uma distribuição unimodal, sendo assim chamados de métodos unimodais. Da mesma forma, métodos multimodais modelam o estado como uma distribuição multimodal. Problemas difíceis, como a navegação de veículos subaquáticos autónomos (AUVs) baseada em localização acústica, frequentemente envolvem outliers recorrentes. Nestas situações, a hipótese correta apenas surge como a mais provável quando um número substancial de medidas é considerado. Soluções robustas para estes problemas precisam de considerar múltiplas hipóteses simultaneamente, o que, por sua vez, exige a representação de distribuições multimodais. Neste trabalho, é apresentado um novo método de inferência aproximada, chamado Gaussian mixture sum-product algorithm (GM-SPA), pois implementa o sum-product algorithm (SPA) para misturas Gaussianas. O GM-SPA pode representar exatamente medidas lineares sub-determinadas e aproximar modelos não lineares importantes, como medidas de distância e cinemática de pose 2D. A robustez a outliers do GM-SPA é testada e comparada com o filtro de partículas (PF) e com multimodal incremental smoothing and mapping (MM- -iSAM), ambos métodos não-paramétricos. A robustez, a exatidão e o tempo de execução em testes de simulação são melhorados. Os problemas de teste incluem localização 1D com associação de dados desconhecida, rastreamento linear de alvos em 3D com outliers correlacionados e estimação de pose 2D com base em medidas de distância com ruído de mistura Gaussiana.

Autonomous navigation has the opportunity to make roads safer and help perform search and rescue missions by reducing human error. Odometry methods are essential to allow for autonomous navigation because they estimate how the robot will move based on the available sensors. This thesis aims to compare and evaluate the Cubature Kalman filter (CKF) based approach for visual-inertial odometry (VIO) to traditional Extended Kalman Filter (EKF) based methods on criteria such as the accuracy of the results. VIO methods use camera and IMU sensor for the predictions. The Multi-State-Constraint Kalman filter (MSCKF) was utilized as the foundation VIO approach to evaluate the underlying filter between EKF and CKF while maintaining the background conditions like visual tracking pipeline, IMU model, and measurement model constant. Evaluation metrics of absolute trajectory error (ATE) and relative error (RE) was used after tuning the filters on EuRoC and KAIST datasets. It is shown that, based on the existing implementation, the filters have no statistically significant difference in performance when predicting motion estimates, despite the fact that the absolute trajectory error of position for EKF estimation is lower. It is further shown that as the length of the trajectory increases, the estimation error for both filters rises unboundedly. Under the visual inertial framework of MSCKF, the CKF filter, which does not linearize the system, works equally as well as the well-established EKF filter and has the potential to perform better with more accurate nonlinear system and measurement models. / Autonom navigering har möjlighet att göra vägar säkrare och hjälpa till att utföra räddningsuppdrag genom att minska mänskliga fel. Odometrimetoder är viktiga för att möjliggöra autonom navigering eftersom de skattar hur roboten rör sig baserat på tillgängliga sensorer. Detta examensarbete syftar till att utvärdera Cubature Kalman filter (CKF) för visuell tröghetsodometri (VIO) och jämföra med traditionella Extended Kalman Filter (EKF) gällande bland annat noggrannhet. VIO-metoder använder kamera och IMU-sensor för skattningarna. MultiState Constraint Kalmanfiltret (MSCKF) användes som grund VIO-metoden för att utvärdera filteralgoritmerna EKF och CKF, samtidigt som de VIO-specifika delarna så som IMU-modell och mätmodell kunde förbli desamma. Utvärderingen gjordes baserat på absolut banfel (ATE) och relativa fel (RE) på EuRoC- och KAIST-datauppsättningar. Det visas att, baserat på den befintliga implementeringen, har filtren ingen statistiskt signifikant skillnad i prestanda när de förutsäger rörelsen, trots att det absoluta banafelet för positionen för EKF-uppskattning är lägre. Det visas vidare att när längden på banan ökar, ökar uppskattningsfelet för båda filtren obegränsat. Under MSCKFs visuella tröghetsramverk fungerar CKF-filtret, som inte linjäriserar systemet, lika bra som det väletablerade EKF-filtret och har potential att prestera bättre med mer exakta olinjära system och mätmodeller.

A mobile laser scanning (MLS) system equipped with a lidar, inertial navigation system and satellite positioning can be used to reconstruct georeferenced point-based models of the surveyed environments. Ideal reconstruction requires accurate trajectories that are challenging to obtain in forests. Satellite signals are heavily degraded under the forest canopy, while lidar-based positioning is often inefficient due to the forest’s unstructured and complex nature. Most forestry-related solutions compute or improve the trajectory in post-processing, focusing on accuracy rather than the possibility of real-time operation. On the other hand, real-time solutions exist, but they are primarily tested and evaluated in urban environments, and the forest’s effect on them is less known. In this study, high-quality, real-time point-based forest model generation was considered by applying techniques from the field of robotics. Forest data were collected with an MLS system mounted 1) on a stick carried by a person and 2) mounted on a forest harvester while performing thinning operations. The system’s trajectory was computed using lidar-inertial-based smoothing and mapping algorithms with real-time limitations. In addition, satellite measurements were either fused into the smoothing algorithm contributing to the trajectory estimation or were used to georeference the trajectory in a post-processing manner. Collecting reliable reference trajectories is difficult in forests. Therefore, this study mainly contains qualitative and relative evaluation. The results indicate that real-time and onboard processing is feasible for forest data with adequate accuracy. State-of-the-art edge and planar feature-based lidar odometry was the most accurate but could not fully maintain real-time operation. On the other hand, the normal distributions transform-based odometry can maintain fast and constant computation with slightly lower accuracy. Fusing the satellite positioning for the mapping reduced the internal integrity of the reconstructed point cloud models, and it is suggested to use it for post-processed georeferencing instead. / Ett mobilt laserskanningssystem (MLS) utrustat med ett lidar, tröghetsnavigeringssystem och satellitpositionering kan användas för att rekonstruera georefererade punktbaserade modeller av de undersökta miljöerna. Idealisk återuppbyggnad kräver exakta banor som är utmanande att uppnå i skogar. Satellitsignaler är kraftigt försämrade under skogens tak, medan lidarbaserad positionering ofta är ineffektiv på grund av skogens ostrukturerade och komplexa natur. De flesta skogsbruksrelaterade lösningar beräknar eller förbättrar banan i efterbearbetning, med fokus på noggrannhet snarare än möjligheten till drift i realtid. Å andra sidan finns realtidslösningar, men de är främst testade och utvärderade i stadsmiljöer och skogens påverkan på dem är mindre känd. I denna studie övervägdes högkvalitativ, punktbaserad skogsmodellgenerering i realtid genom att tillämpa tekniker från robotteknikområdet. Skogsdata samlades in med ett MLS-system monterat 1) på en pinne som bärs av en person och 2) monterad på en skogsskördare under gallringsoperationer. Systemets bana beräknades med hjälp av lidar-tröghetsbaserade utjämnings- och kartläggningsalgoritmer med realtidsbegränsningar. Dessutom fuserades satellitmätningar antingen in i utjämningsalgoritmen som bidrog till banuppskattningen eller användes för att georeferera banan på ett efterbearbetningssätt. Att samla pålitliga referensbanor är svårt i skogar. Därför innehåller denna studie främst kvalitativ och relativ utvärdering. Resultaten indikerar att bearbetning i realtid och ombord är möjlig för skogsdata med tillräcklig noggrannhet. Toppmodern kant- och planfunktionsbaserad lidarodometri var den mest exakta men kunde inte helt upprätthålla realtidsdrift. Å andra sidan kan normalfördelningstransformationsbaserad odometri upprätthålla snabb och konstant beräkning med något lägre noggrannhet. Att sammansmälta satellitpositioneringen för kartläggningen minskade den interna integriteten hos de rekonstruerade punktmolnmodellerna, och det föreslås att man istället använder den för efterbehandlad georeferens.