Exposed ExtendedKalmanFilter to Python and added ports of easyPoint2KalmanFilter and elaboratePoint2KalmanFilter notebooks
mkielo3
parent
f401a9056e
commit
31c84311c8
|
|
@ -725,5 +725,22 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
|
||||||
VALUE calculateEstimate(size_t key) const;
|
VALUE calculateEstimate(size_t key) const;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#include <gtsam/nonlinear/ExtendedKalmanFilter.h>
|
||||||
|
template <T = {gtsam::Point2,
|
||||||
|
gtsam::Point3,
|
||||||
|
gtsam::Rot2,
|
||||||
|
gtsam::Rot3,
|
||||||
|
gtsam::Pose2,
|
||||||
|
gtsam::Pose3,
|
||||||
|
gtsam::NavState,
|
||||||
|
gtsam::imuBias::ConstantBias}>
|
||||||
|
virtual class ExtendedKalmanFilter {
|
||||||
|
ExtendedKalmanFilter(gtsam::Key key_initial, const T& x_initial, const gtsam::noiseModel::Gaussian* P_initial);
|
||||||
|
|
||||||
|
T predict(const gtsam::NoiseModelFactor& motionFactor);
|
||||||
|
T update(const gtsam::NoiseModelFactor& measurementFactor);
|
||||||
|
|
||||||
|
gtsam::JacobianFactor::shared_ptr Density() const;
|
||||||
|
};
|
||||||
|
|
||||||
} // namespace gtsam
|
} // namespace gtsam
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,122 @@
|
||||||
|
{
|
||||||
|
"cells": [
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 1,
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [],
|
||||||
|
"source": [
|
||||||
|
"\"\"\"\n",
|
||||||
|
"Extended Kalman filter on a moving 2D point, but done using factor graphs.\n",
|
||||||
|
"This example uses the ExtendedKalmanFilter class to perform filtering\n",
|
||||||
|
"on a linear system, demonstrating the same operations as in elaboratePoint2KalmanFilter.\n",
|
||||||
|
"\"\"\"\n",
|
||||||
|
"\n",
|
||||||
|
"import gtsam\n",
|
||||||
|
"import numpy as np"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 2,
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [],
|
||||||
|
"source": [
|
||||||
|
"# Create the Kalman Filter initialization point\n",
|
||||||
|
"X0 = gtsam.Point2(0.0, 0.0)\n",
|
||||||
|
"P0 = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1]))\n",
|
||||||
|
"\n",
|
||||||
|
"# Create Key for initial pose\n",
|
||||||
|
"x0 = gtsam.symbol('x', 0)\n",
|
||||||
|
"\n",
|
||||||
|
"# Create an ExtendedKalmanFilter object\n",
|
||||||
|
"ekf = gtsam.ExtendedKalmanFilterPoint2(x0, X0, P0)\n",
|
||||||
|
"\n",
|
||||||
|
"# For this example, we use a constant-position model where\n",
|
||||||
|
"# controls drive the point to the right at 1 m/s\n",
|
||||||
|
"# F = [1 0; 0 1], B = [1 0; 0 1], and u = [1; 0]\n",
|
||||||
|
"# Process noise Q = [0.1 0; 0 0.1]\n",
|
||||||
|
"Q = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1]), True)\n",
|
||||||
|
"\n",
|
||||||
|
"# Measurement noise, assuming a GPS-like sensor\n",
|
||||||
|
"R = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.25, 0.25]), True)\n",
|
||||||
|
"\n",
|
||||||
|
"# Motion model - move right by 1.0 units\n",
|
||||||
|
"dX = gtsam.Point2(1.0, 0.0)\n",
|
||||||
|
"\n",
|
||||||
|
"last_symbol = x0"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 3,
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [
|
||||||
|
{
|
||||||
|
"name": "stdout",
|
||||||
|
"output_type": "stream",
|
||||||
|
"text": [
|
||||||
|
"X1 Predict: [1. 0.]\n",
|
||||||
|
"X1 Update: [1. 0.]\n",
|
||||||
|
"X2 Predict: [2. 0.]\n",
|
||||||
|
"X2 Update: [2. 0.]\n",
|
||||||
|
"X3 Predict: [3. 0.]\n",
|
||||||
|
"X3 Update: [3. 0.]\n",
|
||||||
|
"\n",
|
||||||
|
"Easy Final Covariance (after update):\n",
|
||||||
|
" [[0.0193 0. ]\n",
|
||||||
|
" [0. 0.0193]]\n"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"source": [
|
||||||
|
"for i in range(1, 4):\n",
|
||||||
|
" # Create symbol for new state\n",
|
||||||
|
" xi = gtsam.symbol('x', i)\n",
|
||||||
|
" \n",
|
||||||
|
" # Prediction step: P(x_i) ~ P(x_i|x_{i-1}) P(x_{i-1})\n",
|
||||||
|
" # In Kalman Filter notation: x_{t+1|t} and P_{t+1|t}\n",
|
||||||
|
" motion = gtsam.BetweenFactorPoint2(last_symbol, xi, dX, Q)\n",
|
||||||
|
" Xi_predict = ekf.predict(motion)\n",
|
||||||
|
" print(f\"X{i} Predict:\", Xi_predict)\n",
|
||||||
|
" \n",
|
||||||
|
" # Update step: P(x_i|z_i) ~ P(z_i|x_i)*P(x_i)\n",
|
||||||
|
" # Assuming a measurement model h(x_{t}) = H*x_{t} + v\n",
|
||||||
|
" # where H is the observation model/matrix and v is noise with covariance R\n",
|
||||||
|
" measurement = gtsam.Point2(float(i), 0.0)\n",
|
||||||
|
" meas_factor = gtsam.PriorFactorPoint2(xi, measurement, R)\n",
|
||||||
|
" Xi_update = ekf.update(meas_factor)\n",
|
||||||
|
" print(f\"X{i} Update:\", Xi_update)\n",
|
||||||
|
" \n",
|
||||||
|
" # Move to next state\n",
|
||||||
|
" last_symbol = xi\n",
|
||||||
|
"\n",
|
||||||
|
"A = ekf.Density().getA()\n",
|
||||||
|
"information_matrix = A.transpose() @ A\n",
|
||||||
|
"covariance_matrix = np.linalg.inv(information_matrix)\n",
|
||||||
|
"print (\"\\nEasy Final Covariance (after update):\\n\", covariance_matrix)"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"metadata": {
|
||||||
|
"kernelspec": {
|
||||||
|
"display_name": "Python 3",
|
||||||
|
"language": "python",
|
||||||
|
"name": "python3"
|
||||||
|
},
|
||||||
|
"language_info": {
|
||||||
|
"codemirror_mode": {
|
||||||
|
"name": "ipython",
|
||||||
|
"version": 3
|
||||||
|
},
|
||||||
|
"file_extension": ".py",
|
||||||
|
"mimetype": "text/x-python",
|
||||||
|
"name": "python",
|
||||||
|
"nbconvert_exporter": "python",
|
||||||
|
"pygments_lexer": "ipython3",
|
||||||
|
"version": "3.10.12"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"nbformat": 4,
|
||||||
|
"nbformat_minor": 2
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,191 @@
|
||||||
|
{
|
||||||
|
"cells": [
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 1,
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [],
|
||||||
|
"source": [
|
||||||
|
"\"\"\"\n",
|
||||||
|
"Simple linear Kalman filter on a moving 2D point using factor graphs in GTSAM.\n",
|
||||||
|
"This example manually creates all of the needed data structures to show how\n",
|
||||||
|
"the Kalman filter works under the hood using factor graphs, but uses a loop\n",
|
||||||
|
"to handle the repetitive prediction and update steps.\n",
|
||||||
|
"\n",
|
||||||
|
"Based on the C++ example by Frank Dellaert and Stephen Williams\n",
|
||||||
|
"\"\"\"\n",
|
||||||
|
"\n",
|
||||||
|
"import gtsam\n",
|
||||||
|
"import numpy as np\n",
|
||||||
|
"from gtsam import Point2, noiseModel\n",
|
||||||
|
"from gtsam.symbol_shorthand import X"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 2,
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [],
|
||||||
|
"source": [
|
||||||
|
"# [code below basically does SRIF with Cholesky]\n",
|
||||||
|
"\n",
|
||||||
|
"# Setup containers for linearization points\n",
|
||||||
|
"linearization_points = gtsam.Values()\n",
|
||||||
|
"\n",
|
||||||
|
"# Initialize state x0 at origin\n",
|
||||||
|
"x_initial = Point2(0, 0)\n",
|
||||||
|
"p_initial = noiseModel.Isotropic.Sigma(2, 0.1)\n",
|
||||||
|
"\n",
|
||||||
|
"# Add x0 to linearization points\n",
|
||||||
|
"linearization_points.insert(X(0), x_initial)\n",
|
||||||
|
"\n",
|
||||||
|
"# Initial factor graph with prior on X(0)\n",
|
||||||
|
"gfg = gtsam.GaussianFactorGraph()\n",
|
||||||
|
"ordering = gtsam.Ordering()\n",
|
||||||
|
"ordering.push_back(X(0))\n",
|
||||||
|
"gfg.add(X(0), p_initial.R(), np.zeros(2), noiseModel.Unit.Create(2))\n",
|
||||||
|
"\n",
|
||||||
|
"# Common parameters for all steps\n",
|
||||||
|
"motion_delta = Point2(1, 0) # Always move 1 unit to the right\n",
|
||||||
|
"process_noise = noiseModel.Isotropic.Sigma(2, 0.1)\n",
|
||||||
|
"measurement_noise = noiseModel.Isotropic.Sigma(2, 0.25)"
|
||||||
|
]
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"cell_type": "code",
|
||||||
|
"execution_count": 3,
|
||||||
|
"metadata": {},
|
||||||
|
"outputs": [
|
||||||
|
{
|
||||||
|
"name": "stdout",
|
||||||
|
"output_type": "stream",
|
||||||
|
"text": [
|
||||||
|
"X1 Predict: [1. 0.]\n",
|
||||||
|
"X1 Update: [1. 0.]\n",
|
||||||
|
"X2 Predict: [2. 0.]\n",
|
||||||
|
"X2 Update: [2. 0.]\n",
|
||||||
|
"X3 Predict: [3. 0.]\n",
|
||||||
|
"X3 Update: [3. 0.]\n",
|
||||||
|
"\n",
|
||||||
|
"Elaborate Final Covariance (after update):\n",
|
||||||
|
" [[0.0193 0. ]\n",
|
||||||
|
" [0. 0.0193]]\n"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"source": [
|
||||||
|
"# Current state and conditional\n",
|
||||||
|
"current_x = X(0)\n",
|
||||||
|
"current_conditional = None\n",
|
||||||
|
"current_result = None\n",
|
||||||
|
"\n",
|
||||||
|
"# Run three predict-update cycles\n",
|
||||||
|
"for step in range(1, 4):\n",
|
||||||
|
" # =====================================================================\n",
|
||||||
|
" # Prediction step\n",
|
||||||
|
" # =====================================================================\n",
|
||||||
|
" next_x = X(step)\n",
|
||||||
|
" \n",
|
||||||
|
" # Create new graph with prior from previous step if not the first step\n",
|
||||||
|
" if step > 1:\n",
|
||||||
|
" gfg = gtsam.GaussianFactorGraph()\n",
|
||||||
|
" gfg.add(\n",
|
||||||
|
" current_x,\n",
|
||||||
|
" current_conditional.R(),\n",
|
||||||
|
" current_conditional.d() - current_conditional.R() @ current_result.at(current_x),\n",
|
||||||
|
" current_conditional.get_model()\n",
|
||||||
|
" )\n",
|
||||||
|
" \n",
|
||||||
|
" # Add next state to ordering and create motion model\n",
|
||||||
|
" ordering = gtsam.Ordering()\n",
|
||||||
|
" ordering.push_back(current_x)\n",
|
||||||
|
" ordering.push_back(next_x)\n",
|
||||||
|
" \n",
|
||||||
|
" # Create motion factor and add to graph\n",
|
||||||
|
" motion_factor = gtsam.BetweenFactorPoint2(current_x, next_x, motion_delta, process_noise)\n",
|
||||||
|
" \n",
|
||||||
|
" # Add next state to linearization points if this is the first step\n",
|
||||||
|
" if step == 1:\n",
|
||||||
|
" linearization_points.insert(next_x, x_initial)\n",
|
||||||
|
" else:\n",
|
||||||
|
" linearization_points.insert(next_x, \n",
|
||||||
|
" linearization_points.atPoint2(current_x))\n",
|
||||||
|
" \n",
|
||||||
|
" # Add linearized factor to graph\n",
|
||||||
|
" gfg.push_back(motion_factor.linearize(linearization_points))\n",
|
||||||
|
" \n",
|
||||||
|
" # Solve for prediction\n",
|
||||||
|
" prediction_bayes_net = gfg.eliminateSequential(ordering)\n",
|
||||||
|
" next_conditional = prediction_bayes_net.back()\n",
|
||||||
|
" prediction_result = prediction_bayes_net.optimize()\n",
|
||||||
|
" \n",
|
||||||
|
" # Extract and store predicted state\n",
|
||||||
|
" next_predict = linearization_points.atPoint2(next_x) + Point2(prediction_result.at(next_x))\n",
|
||||||
|
" print(f\"X{step} Predict:\", next_predict)\n",
|
||||||
|
" linearization_points.update(next_x, next_predict)\n",
|
||||||
|
" \n",
|
||||||
|
" # =====================================================================\n",
|
||||||
|
" # Update step\n",
|
||||||
|
" # =====================================================================\n",
|
||||||
|
" # Create new graph with prior from prediction\n",
|
||||||
|
" gfg = gtsam.GaussianFactorGraph()\n",
|
||||||
|
" gfg.add(\n",
|
||||||
|
" next_x,\n",
|
||||||
|
" next_conditional.R(),\n",
|
||||||
|
" next_conditional.d() - next_conditional.R() @ prediction_result.at(next_x),\n",
|
||||||
|
" next_conditional.get_model()\n",
|
||||||
|
" )\n",
|
||||||
|
" \n",
|
||||||
|
" # Create ordering for update\n",
|
||||||
|
" ordering = gtsam.Ordering()\n",
|
||||||
|
" ordering.push_back(next_x)\n",
|
||||||
|
" \n",
|
||||||
|
" # Create measurement at correct position\n",
|
||||||
|
" measurement = Point2(float(step), 0.0)\n",
|
||||||
|
" meas_factor = gtsam.PriorFactorPoint2(next_x, measurement, measurement_noise)\n",
|
||||||
|
" \n",
|
||||||
|
" # Add measurement factor to graph\n",
|
||||||
|
" gfg.push_back(meas_factor.linearize(linearization_points))\n",
|
||||||
|
" \n",
|
||||||
|
" # Solve for update\n",
|
||||||
|
" update_bayes_net = gfg.eliminateSequential(ordering)\n",
|
||||||
|
" current_conditional = update_bayes_net.back()\n",
|
||||||
|
" current_result = update_bayes_net.optimize()\n",
|
||||||
|
" \n",
|
||||||
|
" # Extract and store updated state\n",
|
||||||
|
" next_update = linearization_points.atPoint2(next_x) + Point2(current_result.at(next_x))\n",
|
||||||
|
" print(f\"X{step} Update:\", next_update)\n",
|
||||||
|
" linearization_points.update(next_x, next_update)\n",
|
||||||
|
" \n",
|
||||||
|
" # Move to next state\n",
|
||||||
|
" current_x = next_x\n",
|
||||||
|
"\n",
|
||||||
|
"final_R = current_conditional.R()\n",
|
||||||
|
"final_information = final_R.transpose() @ final_R\n",
|
||||||
|
"final_covariance = np.linalg.inv(final_information)\n",
|
||||||
|
"print(\"\\nElaborate Final Covariance (after update):\\n\", final_covariance)"
|
||||||
|
]
|
||||||
|
}
|
||||||
|
],
|
||||||
|
"metadata": {
|
||||||
|
"kernelspec": {
|
||||||
|
"display_name": "Python 3",
|
||||||
|
"language": "python",
|
||||||
|
"name": "python3"
|
||||||
|
},
|
||||||
|
"language_info": {
|
||||||
|
"codemirror_mode": {
|
||||||
|
"name": "ipython",
|
||||||
|
"version": 3
|
||||||
|
},
|
||||||
|
"file_extension": ".py",
|
||||||
|
"mimetype": "text/x-python",
|
||||||
|
"name": "python",
|
||||||
|
"nbconvert_exporter": "python",
|
||||||
|
"pygments_lexer": "ipython3",
|
||||||
|
"version": "3.10.12"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"nbformat": 4,
|
||||||
|
"nbformat_minor": 2
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue