From 628f152dcfd52ec70f68a21ae371f42b383d680b Mon Sep 17 00:00:00 2001 From: Thibaud Teil Date: Sat, 24 Jun 2023 14:16:06 -0600 Subject: [PATCH 1/3] Add position OD uKF module --- .../positionODuKF/positionODuKF.cpp | 507 ++++++++++++++++++ .../positionODuKF/positionODuKF.h | 119 ++++ .../positionODuKF/positionODuKF.i | 45 ++ 3 files changed, 671 insertions(+) create mode 100644 src/fswAlgorithms/opticalNavigation/positionODuKF/positionODuKF.cpp create mode 100644 src/fswAlgorithms/opticalNavigation/positionODuKF/positionODuKF.h create mode 100644 src/fswAlgorithms/opticalNavigation/positionODuKF/positionODuKF.i diff --git a/src/fswAlgorithms/opticalNavigation/positionODuKF/positionODuKF.cpp b/src/fswAlgorithms/opticalNavigation/positionODuKF/positionODuKF.cpp new file mode 100644 index 0000000000..0d8d20808c --- /dev/null +++ b/src/fswAlgorithms/opticalNavigation/positionODuKF/positionODuKF.cpp @@ -0,0 +1,507 @@ +/* + ISC License + + Copyright (c) 2023, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "positionODuKF.h" + +PositionODuKF::PositionODuKF() = default; + +PositionODuKF::~PositionODuKF() = default; + +/*! Reset the position-based OD filter to an initial state and + initializes the internal estimation matrices. + @return void + @param CurrentSimNanos The clock time at which the function was called (nanoseconds) + */ +void PositionODuKF::Reset(uint64_t CurrentSimNanos) +{ + /*! - Check if the required message has not been connected */ + if (!this->cameraPosMsg.isLinked()) { + bskLogger.bskLog(BSK_ERROR, "Error: positionODuKF opNavUnitVecInMsg wasn't connected."); + } + + /*! - Initialize filter parameters and change units to km and s */ + this->muCentral *= 1E-9; // mu is input in meters + this->state = 1E-3 * this->stateInitial; + this->sBar = 1E-6 * this->covarInitial; + this->covar = 1E-6 * this->covarInitial; + this->covar.resize(this->state.size(), this->state.size()); + this->sBar.resize(this->state.size(), this->state.size()); + this->measurementNoise.resize(this->obs.size(), this->obs.size()); + this->processNoise.resize(this->state.size(), this->state.size()); + + this->previousFilterTimeTag = (double) CurrentSimNanos*NANO2SEC; + this->numberSigmaPoints = this->state.size()*2+1; + this->dt = 0; + + /*! - Ensure that all internal filter matrices are zeroed*/ + this->obs.setZero(this->state.size()); + this->wM.setZero(this->numberSigmaPoints); + this->wC.setZero(this->numberSigmaPoints); + this->sBar.setZero(this->state.size(), this->state.size()); + this->sigmaPoints.setZero(this->state.size(), this->numberSigmaPoints); + this->cholProcessNoise.setZero(this->state.size(), this->state.size()); + + /*! - Set lambda/gamma to standard value for unscented kalman filters */ + this->lambda = (double) this->state.size()*(this->alpha*this->alpha - 1); + this->eta = sqrt((double) this->state.size() + this->lambda); + + /*! - Set the wM/wC vectors to standard values for unscented kalman filters*/ + this->wM(0) = this->lambda / ((double) this->state.size() + this->lambda); + this->wC(0) = this->lambda / ((double) this->state.size() + this->lambda) + (1 - this->alpha*this->alpha + this->beta); + for (size_t i = 1; i < this->numberSigmaPoints; i++) + { + this->wM(i) = 1.0 / (2.0 * ((double) this->state.size() + this->lambda)); + this->wC(i) = this->wM(i); + } + + /*! - Perform cholesky decompositions of covariance and noise */ + this->sBar = PositionODuKF::choleskyDecomposition(this->covar); + this->cholProcessNoise = PositionODuKF::choleskyDecomposition(this->processNoise * 1E-6); +} + +/*! Take the relative position measurements and outputs an estimate of the + spacecraft states in the inertial frame. + @return void + @param CurrentSimNanos The clock time at which the function was called (nanoseconds) + */ +void PositionODuKF::UpdateState(uint64_t CurrentSimNanos) +{ + this->readFilterMeasurements(); + /*! - If the time tag from the measured data is new compared to previous step, + propagate and update the filter*/ + if(this->cameraPosBuffer.timeTag * NANO2SEC >= this->previousFilterTimeTag && this->cameraPosBuffer.valid) + { + this->timeUpdate(this->cameraPosBuffer.timeTag * NANO2SEC); + this->measurementUpdate(); + this->computePostFitResiduals(); + } + /*! - If current clock time is further ahead than the measured time, then + propagate to this current time-step*/ + if((double) CurrentSimNanos*NANO2SEC >= this->previousFilterTimeTag) + { + this->timeUpdate((double) CurrentSimNanos * NANO2SEC); + } + + this->writeOutputMessages(CurrentSimNanos); +} + +/*! Perform the time update for the position OD kalman filter. + It propagates the sigma points forward in time and then gets the current + covariance and state estimates. + @return void + @param updateTime The time that we need to fix the filter to (seconds) + */ +void PositionODuKF::timeUpdate(double updateTime) +{ + Eigen::VectorXd propagatedSigmaPoint; + Eigen::MatrixXd A(this->state.size(), 3*this->state.size()); + + this->dt = updateTime - this->previousFilterTimeTag; + std::array time = {0, this->dt}; + + /*! - Copy over the current state estimate into the 0th Sigma point and propagate by dt*/ + this->sigmaPoints.col(0) = propagate(time, this->state, this->dt); + /*! - Scale that Sigma point by the appopriate scaling factor (Wm[0])*/ + this->xBar = this->wM(0)*this->sigmaPoints.col(0); + + /*! - For each Sigma point, apply sBar-based error, propagate forward, and scale by Wm just like 0th. + Note that we perform +/- sigma points simultaneously in loop to save loop values.*/ + for (size_t i = 1; istate.size() + 1; i++) + { + /*! - Adding covariance columns from sigma points*/ + this->sigmaPoints.col(i) = propagate(time, this->state + this->eta * this->sBar.col(i-1), this->dt); + /*! - Subtracting covariance columns from sigma points*/ + this->sigmaPoints.col( i + this->state.size()) = + propagate(time, this->state - this->eta * this->sBar.col(i-1), this->dt); + } + + /*! - Compute xbar according to Eq (19)*/ + for (size_t i = 1; inumberSigmaPoints; i++) + { + this->xBar += this->wM(i)*this->sigmaPoints.col(i); + } + + /*! - Assemble the A matrix for QR decomposition as seen in equation 20 in the reference document*/ + for (size_t i = 1; i < this->numberSigmaPoints; i++) + { + A.col(i-1) = sqrt(this->wC(i))*(this->sigmaPoints.col(i) - this->xBar); + } + + A.block(0, this->numberSigmaPoints-1, this->state.size(), this->state.size()) = this->cholProcessNoise; + + /*! - QR decomposition (only R is of interest) of the A matrix provides the new sBar matrix*/ + this->sBar = PositionODuKF::qrDecompositionJustR(A); + + /*! - Shift the sBar matrix over by the xBar vector using the appropriate weight + like in equation 21 in design document.*/ + Eigen::VectorXd xError = this->sigmaPoints.col(0) - this->xBar; + + /*! - Cholesky update block for vectors.*/ + this->sBar = PositionODuKF::choleskyUpDownDate(this->sBar, xError, this->wC(0)); + + this->covar = this->sBar*this->sBar.transpose(); + this->state = this->sigmaPoints.col(0); + this->previousFilterTimeTag = updateTime; +} + + +/*! Read the message containing the measurement data. + * It updates class variables relating to measurement data including validity and time tags. + @return void + */ +void PositionODuKF::writeOutputMessages(uint64_t CurrentSimNanos) { + this->opNavFilterMsgBuffer = this->opNavFilterMsg.zeroMsgPayload; + this->navTransOutMsgBuffer = this->navTransOutMsg.zeroMsgPayload; + + /*! - Write the position estimate into the copy of the navigation message structure*/ + eigenMatrixXd2CArray(1e3*this->state.head(3), this->navTransOutMsgBuffer.r_BN_N); + eigenMatrixXd2CArray(1e3*this->state.tail(3), this->navTransOutMsgBuffer.v_BN_N); + + /*! - Populate the filter states output buffer and write the output message*/ + this->opNavFilterMsgBuffer.timeTag = this->previousFilterTimeTag; + eigenMatrixXd2CArray(1e3*this->state, this->opNavFilterMsgBuffer.state); + eigenMatrixXd2CArray(1e3*this->xBar, this->opNavFilterMsgBuffer.stateError); + eigenMatrixXd2CArray(1e6*this->covar, this->opNavFilterMsgBuffer.covar); + + if (this->measurementRead){ + eigenMatrixXd2CArray(1e3*this->postFits, this->opNavFilterMsgBuffer.postFitRes); + } + + this->navTransOutMsg.write(&this->navTransOutMsgBuffer, this->moduleID, CurrentSimNanos); + this->opNavFilterMsg.write(&this->opNavFilterMsgBuffer, this->moduleID, CurrentSimNanos); +} + +/*! Read the message containing the measurement data. + * It updates class variables relating to measurement data including validity and time tags. + @return void + */ +void PositionODuKF::readFilterMeasurements() { + this->cameraPosBuffer = this->cameraPosMsg(); + this->measurementRead = false; + if (this->cameraPosBuffer.valid){ + /*! - Read measurement and cholesky decomposition its noise*/ + this->obs = cArray2EigenVector3d(this->cameraPosBuffer.cameraPos_N)*1E-3; // Change units to km + this->measurementNoise.resize(this->obs.size(), this->obs.size()); + this->measurementNoise << this->measNoiseSD*this->measNoiseSD, 0, 0, + 0, this->measNoiseSD*this->measNoiseSD, 0, + 0, 0, this->measNoiseSD*this->measNoiseSD; + this->measurementNoise *= 1E-6*this->measNoiseScaling; // Change units to km and scale + this->cholMeasurementNoise = PositionODuKF::choleskyDecomposition(this->measurementNoise); + this->measurementRead = true; + } +} + +/*! Compute the measurement model. Given that the data is coming from + the center of brightness Converter, the transformation has already taken place from pixel data + to spacecraft position. + @return void + */ +void PositionODuKF::measurementModel() +{ + this->yMeas.setZero(3, this->numberSigmaPoints); + for(size_t j=0; j < this->numberSigmaPoints; j++) + { + /*! Sigma points positions need to be normalized for the measurement model.*/ + this->yMeas.col(j) = this->sigmaPoints.col(j).head(3); + } +} + +/*! Compute the post fit residuals if the measurement data was fresh. + * The post fits are y - ybar if a measurement was read, if observations are not present + * a flag is raised to not compute post fit residuals +@return void + */ +void PositionODuKF::computePostFitResiduals() +{ + /*! - Compute Post Fit Residuals, first get Y (eq 22) using the states post fit*/ + this->measurementModel(); + /*! - Compute the value for the yBar parameter (equation 23)*/ + this->postFits.setZero(this->obs.size()); + Eigen::VectorXd yBar; + yBar.setZero(this->obs.size()); + for(size_t i=0; inumberSigmaPoints; i++){ + yBar += this->wM(i)*this->yMeas.col(i); + } + this->postFits = this->obs - yBar; +} + +/*! Perform the measurement update for the kalman filter. + It applies the observations in the obs vectors to the current state estimate and + updates the state/covariance with that information. + @return void + */ +void PositionODuKF::measurementUpdate() +{ + /*! - Compute the valid observations and the measurement model for all observations*/ + this->measurementModel(); + + /*! - Compute the value for the yBar parameter (note that this is equation 23 in the + time update section of the reference document*/ + Eigen::VectorXd yBar; + yBar.setZero(this->obs.size()); + for(size_t i=0; inumberSigmaPoints; i++) + { + yBar += this->wM(i) * this->yMeas.col(i); + } + + /*! - Populate the matrix that we perform the QR decomposition on in the measurement + update section of the code. This is based on the difference between the yBar + parameter and the calculated measurement models. Equation 24. */ + Eigen::MatrixXd A(this->obs.size(), 2*this->state.size() + this->obs.size()); + for(size_t i=1; inumberSigmaPoints; i++) + { + A.col(i-1) = sqrt(this->wC(1))*(yMeas.col(i) - yBar); + } + A.block(0, this->numberSigmaPoints-1, this->obs.size(), this->obs.size()) = + this->cholMeasurementNoise; + + /*! - Perform QR decomposition (only R again) of the above matrix to obtain the + current Sy matrix*/ + Eigen::MatrixXd sy; + sy.setZero(this->obs.size(), this->obs.size()); + sy = PositionODuKF::qrDecompositionJustR(A); + + /*! - Cholesky update block for vectors.*/ + Eigen::VectorXd yError = this->yMeas.col(0) - yBar; + sy = PositionODuKF::choleskyUpDownDate(sy, yError, this->wC(0)); + + /*! - Construct the Pxy matrix (equation 26) which multiplies the Sigma-point cloud + by the measurement model cloud (weighted) to get the total Pxy matrix*/ + Eigen::VectorXd xError; + xError.setZero(this->state.size()); + Eigen::MatrixXd kMat; + kMat.setZero(this->state.size(), this->obs.size()); + Eigen::MatrixXd STkMatT; + STkMatT.setZero(this->obs.size(), this->state.size()); + Eigen::MatrixXd pXY; + pXY.setZero(this->state.size(), this->obs.size()); + + for(size_t i=0; inumberSigmaPoints; i++) + { + xError = this->sigmaPoints.col(i) - this->xBar; + yError = this->yMeas.col(i) - yBar; + kMat = this->wC(i) * xError * yError.transpose(); + pXY += kMat; + } + + /*! - Then we need to invert the SyT*Sy matrix to get the Kalman gain factor. Since + The Sy matrix is lower triangular, we can do a back-sub inversion instead of + a full matrix inversion. Equation 27 in the reference document.*/ + STkMatT = PositionODuKF::forwardSubstitution(sy, pXY.transpose()); + kMat = PositionODuKF::backSubstitution(sy.transpose(), STkMatT).transpose(); + + /*! - Difference the yBar and the observations to get the observed error and + multiply by the Kalman Gain to get the state update. Add the state update + to the state to get the updated state value (equation 27).*/ + this->state = this->xBar + kMat*(this->obs - yBar); + + /*! - Compute the updated matrix U from equation 28 */ + Eigen::MatrixXd Umat; + Umat.setZero(this->state.size(), this->obs.size()); + Umat = kMat * sy; + + /*! - For each column in the update matrix, perform a cholesky down-date on it to + get the total shifted S matrix (called sBar in internal parameters*/ + for(int i=0; i < Umat.cols(); i++) + { + this->sBar = PositionODuKF::choleskyUpDownDate(this->sBar, Umat.col(i), -1); + } + + /*! - Compute equivalent covariance based on updated sBar matrix*/ + this->covar = this->sBar*this->sBar.transpose(); +} + +/*! Integrate the equations of motion of two body point mass gravity using Runge-Kutta 4 (RK4) + @param interval integration interval + @param X0 initial state + @param dt time step + @return Eigen::VectorXd +*/ +Eigen::VectorXd PositionODuKF::propagate(std::array interval, const Eigen::VectorXd& X0, double dt) const +{ + double t_0 = interval[0]; + double t_f = interval[1]; + double t = t_0; + Eigen::VectorXd X = X0; + + std::function f = [this](double t, Eigen::VectorXd state) + { + Eigen::VectorXd stateDerivative(state.size()); + /*! Implement point mass gravity for the propagation */ + stateDerivative.segment(0,3) = state.segment(3, 3); + stateDerivative.segment(3,3) = - this->muCentral/(pow(state.head(3).norm(),3)) * state.head(3); + + return stateDerivative; + }; + + /*! Propagate to t_final with an RK4 integrator */ + double N = ceil((t_f-t_0)/dt); + for (int c=0; c < N; c++) { + double step = std::min(dt,t_f-t); + X = this->rk4(f, X, t, step); + t = t + step; + } + + return X; +} + +/*! Runge-Kutta 4 (RK4) function for the state propagation + @param ODEfunction function handle that includes the equations of motion + @param X0 initial state + @param t0 initial time + @param dt time step + @return Eigen::VectorXd +*/ +Eigen::VectorXd PositionODuKF::rk4(const std::function& ODEfunction, + const Eigen::VectorXd& X0, + double t0, + double dt) const +{ + double h = dt; + + Eigen::VectorXd k1 = ODEfunction(t0, X0); + Eigen::VectorXd k2 = ODEfunction(t0 + h/2., X0 + h*k1/2.); + Eigen::VectorXd k3 = ODEfunction(t0 + h/2., X0 + h*k2/2.); + Eigen::VectorXd k4 = ODEfunction(t0 + h, X0 + h*k3); + + Eigen::VectorXd X = X0 + 1./6.*h*(k1 + 2.*k2 + 2.*k3 + k4); + + return X; +} + +/*! Perform a QR decomposition using HouseholderQR from Eigen, but only returns the transpose of the + * upper triangular R matrix truncated such that it is the same size as the input matrix. + @return Eigen::MatrixXd + @param Eigen::MatrixXd input : The input matrix. If not square, provide it with more cols then rows + */ +Eigen::MatrixXd PositionODuKF::qrDecompositionJustR(const Eigen::MatrixXd& input) const +{ + Eigen::HouseholderQR qrDecomposition(input.transpose()); + Eigen::MatrixXd R_tilde; + R_tilde.setZero(input.rows(), input.rows()); + Eigen::MatrixXd R; + R.setZero(input.cols(), input.rows()); + + /*! Use Eigen Householder method to perform a QR decomposition and retrieve just the R matrix. + * Math is described in the first bullet of section 3 page 3 of the reference document */ + Eigen::MatrixXd Q; + Q = qrDecomposition.householderQ(); + R = Q.transpose()*input.transpose(); + R_tilde = R.block(0,0,input.rows(), input.rows()); + + /*! Zero all terms that should be zero to avoid errors accumulating */ + for (int i =0; i < R_tilde.rows(); i ++){ + for (int j = 0 ; j < i; j ++){ + R_tilde(i, j) = 0; + } + } + + return R_tilde.transpose(); +} + +/*! Perform the cholesky up or down date. The sign of the value term determines weather to update or downdate + * the input matrix using the input vector. + * Return the QR decomposed matrix of the up/down dated input matrix. + @return Eigen::MatrixXd + @param Eigen::MatrixXd input : The input matrix which is recomposed into a P matrix P = S.S^T + @param Eigen::VectorXd inputVector : Take it's outer product V.V^T to add into the recomposed P matrix + @param Eigen::VectorXd coefficient : Factor that is square rooted and scales the outer product P +/- sqrt(v)V.V^T + */ +Eigen::MatrixXd PositionODuKF::choleskyUpDownDate(const Eigen::MatrixXd& input, + const Eigen::VectorXd& inputVector, + double coefficient) const +{ + Eigen::MatrixXd P; + P.setZero(inputVector.size(), inputVector.size()); + + /*! Perform the Cholesky factor updating. + * Math is described in the second bullet of section 3 page 3 of the reference document */ + P = input * input.transpose(); + P += ((coefficient > 0) - (coefficient < 0))*abs(coefficient)*inputVector*inputVector.transpose(); + + Eigen::MatrixXd A; + A = PositionODuKF::choleskyDecomposition(P); + return qrDecompositionJustR(A); +} + +/*! Perform the cholesky decomposition of an input matrix using Eigen's LLT + @return Eigen::MatrixXd + @param Eigen::MatrixXd input : The input matrix + */ +Eigen::MatrixXd PositionODuKF::choleskyDecomposition(const Eigen::MatrixXd& input) const +{ + Eigen::LLT choleskyDecomp(input); + return choleskyDecomp.matrixL(); +} + +/*! Perform a generic back substitution to solve x in Ux = b + @return Eigen::MatrixXd + @param Eigen::MatrixXd U, an upper triangular matrix + @param Eigen::MatrixXd b, the right hand side of the Ux = b + */ +Eigen::MatrixXd PositionODuKF::backSubstitution(const Eigen::MatrixXd& U, const Eigen::MatrixXd& b) const +{ + assert(U.rows() == b.rows()); + + Eigen::MatrixXd x; + Eigen::VectorXd xCol; + + x.setZero(b.rows(), b.cols()); + for (int col =0; col < b.cols(); col++){ + xCol.setZero(b.rows()); + for (long i = U.rows()-1; i >= 0; i--){ + xCol(i) = b(i, col); + for (long j = i+1 ; j < U.rows(); j++){ + xCol(i) = xCol(i) - U(i,j)*xCol(j); + } + xCol(i) = xCol(i)/U(i,i); + } + x.col(col) = xCol; + } + + return x; +} + +/*! Perform a generic forward substitution to solve x in Lx = b + @return Eigen::MatrixXd + @param Eigen::MatrixXd L, an lower triangular matrix + @param Eigen::MatrixXd b, the right hand side of the Ux = b + */ +Eigen::MatrixXd PositionODuKF::forwardSubstitution(const Eigen::MatrixXd& L, const Eigen::MatrixXd& b) const +{ + assert(L.rows() == b.rows()); + + Eigen::MatrixXd x; + Eigen::VectorXd xCol; + + x.setZero(b.rows(), b.cols()); + for (int col =0; col < b.cols(); col++){ + xCol.setZero(b.rows()); + for (int i =0; i < L.rows(); i++){ + xCol(i) = b(i, col); + for (int j = 0 ; j < i; j++){ + xCol(i) = xCol(i) - L(i,j)*xCol(j); + } + xCol(i) = xCol(i)/L(i,i); + } + x.col(col) = xCol; + } + + return x; +} diff --git a/src/fswAlgorithms/opticalNavigation/positionODuKF/positionODuKF.h b/src/fswAlgorithms/opticalNavigation/positionODuKF/positionODuKF.h new file mode 100644 index 0000000000..443c6c4252 --- /dev/null +++ b/src/fswAlgorithms/opticalNavigation/positionODuKF/positionODuKF.h @@ -0,0 +1,119 @@ +/* + ISC License + + Copyright (c) 2023, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + + +/*! @brief Top level structure for the position based OD unscented kalman filter. + Used to estimate the spacecraft's inertial position relative to a body. + */ + +#ifndef POSODUKF_H +#define POSODUKF_H + +#include "architecture/_GeneralModuleFiles/sys_model.h" +#include "architecture/utilities/bskLogging.h" +#include "architecture/messaging/messaging.h" +#include "architecture/utilities/orbitalMotion.h" +#include "architecture/utilities/avsEigenSupport.h" +#include "architecture/utilities/macroDefinitions.h" +#include "architecture/msgPayloadDefC/NavTransMsgPayload.h" +#include "architecture/msgPayloadDefCpp/OpNavSUKFMsgPayload.h" +#include "architecture/msgPayloadDefC/CameraLocalizationMsgPayload.h" + +#include +#include +#include + +class PositionODuKF: public SysModel { +public: + PositionODuKF(); + ~PositionODuKF() override; + void Reset(uint64_t CurrentSimNanos) override; + void UpdateState(uint64_t CurrentSimNanos) override; + +private: + void timeUpdate(const double updateTime); + void measurementUpdate(); + void measurementModel(); + void readFilterMeasurements(); + void writeOutputMessages(uint64_t CurrentSimNanos); + void computePostFitResiduals(); + Eigen::MatrixXd qrDecompositionJustR(const Eigen::MatrixXd& input) const; + Eigen::MatrixXd choleskyUpDownDate(const Eigen::MatrixXd& input, + const Eigen::VectorXd& inputVector, + double coefficient) const; + Eigen::MatrixXd choleskyDecomposition(const Eigen::MatrixXd& input) const; + Eigen::MatrixXd backSubstitution(const Eigen::MatrixXd& U, const Eigen::MatrixXd& b) const; + Eigen::MatrixXd forwardSubstitution(const Eigen::MatrixXd& L, const Eigen::MatrixXd& b) const; + + Eigen::VectorXd rk4(const std::function& ODEfunction, + const Eigen::VectorXd& X0, + double t0, + double dt) const; + Eigen::VectorXd propagate(std::array interval, const Eigen::VectorXd& X0, double dt) const; + +public: + ReadFunctor cameraPosMsg; + CameraLocalizationMsgPayload cameraPosBuffer; + Message navTransOutMsg; + Message opNavFilterMsg; + + //!< Variables are named closely to the reference document : + //!< "The Square-root unscented Kalman Filter for state and parameter-estimation" by van der Merwe and Wan + double beta; + double alpha; + double kappa; + double lambda; + double eta; + double muCentral; + + Eigen::MatrixXd processNoise; //!< [-] process noise matrix + Eigen::MatrixXd measurementNoise; //!< [-] Measurement Noise + Eigen::VectorXd stateInitial; //!< [-] State estimate for time TimeTag at previous time + Eigen::MatrixXd sBarInitial; //!< [-] Time updated covariance at previous time + Eigen::MatrixXd covarInitial; //!< [-] covariance at previous time + + double measNoiseScaling = 1; //!< [-] Scale factor that can be applied on the measurement noise to over/under weight + double measNoiseSD = 0.1; //!< [km] Constant measurement noise +private: + NavTransMsgPayload navTransOutMsgBuffer; //!< Message buffer for input translational nav message + OpNavSUKFMsgPayload opNavFilterMsgBuffer; + + double dt; //!< [s] seconds since last data epoch + double previousFilterTimeTag; //!< [s] Time tag for statecovar/etc + bool measurementRead=false; //!< [bool] Presence of a valid measurement to process + size_t numberSigmaPoints; //!< [s] 2n+1 sigma points for convenience + Eigen::VectorXd wM; + Eigen::VectorXd wC; + + Eigen::VectorXd state; //!< [-] State estimate for time TimeTag + Eigen::MatrixXd sBar; //!< [-] Time updated covariance + Eigen::MatrixXd covar; //!< [-] covariance + Eigen::VectorXd xBar; //!< [-] Current mean state estimate + Eigen::MatrixXd sigmaPoints; //!< [-] sigma point matrix + + Eigen::VectorXd obs; //!< [-] Observation vector for frame + Eigen::MatrixXd yMeas; //!< [-] Measurement model data + Eigen::VectorXd postFits; //!< [-] PostFit residuals + Eigen::MatrixXd cholProcessNoise; //!< [-] cholesky of Qnoise + Eigen::MatrixXd cholMeasurementNoise; //!< [-] cholesky of Qnoise + + BSKLogger bskLogger; //!< -- BSK Logging +}; + +#endif diff --git a/src/fswAlgorithms/opticalNavigation/positionODuKF/positionODuKF.i b/src/fswAlgorithms/opticalNavigation/positionODuKF/positionODuKF.i new file mode 100644 index 0000000000..e72c25470a --- /dev/null +++ b/src/fswAlgorithms/opticalNavigation/positionODuKF/positionODuKF.i @@ -0,0 +1,45 @@ +/* + ISC License + + Copyright (c) 2023, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ +%module positionODuKF +%{ + #include "positionODuKF.h" +%} + +%pythoncode %{ +from Basilisk.architecture.swig_common_model import * +%} + +%include "stdint.i" +%include "std_string.i" +%include "sys_model.h" +%include "swig_eigen.i" +%include "swig_conly_data.i" + +%include "positionODuKF.h" + +%include "architecture/msgPayloadDefC/NavTransMsgPayload.h" +struct NavTransMsg_C; +%include "architecture/msgPayloadDefC/CameraLocalizationMsgPayload.h" +struct CameraLocalizationMsg_C; +%include "architecture/msgPayloadDefCpp/OpNavSUKFMsgPayload.h" + +%pythoncode %{ +import sys +protectAllClasses(sys.modules[__name__]) +%} From c568013028aaced69ca5873af22f10647e0a7388 Mon Sep 17 00:00:00 2001 From: Thibaud Teil Date: Sat, 24 Jun 2023 14:16:28 -0600 Subject: [PATCH 2/3] Add unit test for the positionODuKF module --- .../_UnitTest/positionODuKF_test_utilities.py | 225 ++++++++++++++ .../_UnitTest/test_positionODuKF.py | 288 ++++++++++++++++++ 2 files changed, 513 insertions(+) create mode 100644 src/fswAlgorithms/opticalNavigation/positionODuKF/_UnitTest/positionODuKF_test_utilities.py create mode 100644 src/fswAlgorithms/opticalNavigation/positionODuKF/_UnitTest/test_positionODuKF.py diff --git a/src/fswAlgorithms/opticalNavigation/positionODuKF/_UnitTest/positionODuKF_test_utilities.py b/src/fswAlgorithms/opticalNavigation/positionODuKF/_UnitTest/positionODuKF_test_utilities.py new file mode 100644 index 0000000000..13b8848608 --- /dev/null +++ b/src/fswAlgorithms/opticalNavigation/positionODuKF/_UnitTest/positionODuKF_test_utilities.py @@ -0,0 +1,225 @@ +# +# ISC License +# +# Copyright (c) 2023, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder +# +# Permission to use, copy, modify, and/or distribute this software for any +# purpose with or without fee is hereby granted, provided that the above +# copyright notice and this permission notice appear in all copies. +# +# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES +# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF +# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR +# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES +# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN +# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF +# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. +# +import inspect +import os +import sys + +import math +import numpy as np +from Basilisk.utilities import unitTestSupport + +filename = inspect.getframeinfo(inspect.currentframe()).filename +path = os.path.dirname(os.path.abspath(filename)) +splitPath = path.split('fswAlgorithms') + +import matplotlib as mpl +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +from matplotlib.patches import Ellipse + +color_x = 'dodgerblue' +color_y = 'salmon' +color_z = 'lightgreen' +m2km = 1.0 / 1000.0 + +def StatePlot(x, testName, show_plots): + + numStates = len(x[0,:])-1 + + t= np.zeros(len(x[:,0])) + for i in range(len(t)): + t[i] = x[i, 0]*1E-9 + + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.subplot(321) + plt.plot(t , x[:, 1], "b", label='Error Filter') + plt.legend(loc='lower right') + plt.title('First pos component (m)') + plt.grid() + + plt.subplot(322) + plt.plot(t , x[:, 4], "b") + plt.title('Second rate component (m/s)') + plt.grid() + + plt.subplot(323) + plt.plot(t , x[:, 2], "b") + plt.title('Second pos component (m)') + plt.grid() + + plt.subplot(324) + plt.plot(t , x[:, 5], "b") + plt.xlabel('t(s)') + plt.title('Third rate component (m/s)') + plt.grid() + + plt.subplot(325) + plt.plot(t , x[:, 3], "b") + plt.xlabel('t(s)') + plt.title('Third pos component (m)') + plt.grid() + + plt.subplot(326) + plt.plot(t , x[:, 6], "b") + plt.xlabel('t(s)') + plt.title('Third rate component (m/s)') + plt.grid() + + if show_plots: + plt.show() + plt.close() + +def EnergyPlot(t, energy, testName, show_plots): + + conserved= np.zeros(len(t)) + for i in range(len(t)): + conserved[i] = (energy[i] - energy[0])/energy[0] + + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.plot(t , conserved, "b", label='Energy') + plt.legend(loc='lower right') + plt.title('Energy ' + testName) + plt.grid() + + if show_plots: + plt.show() + plt.close() + + +def StateCovarPlot(x, Pflat, testName, show_plots): + + numStates = len(x[0,:])-1 + + P = np.zeros([len(Pflat[:,0]),numStates,numStates]) + t= np.zeros(len(Pflat[:,0])) + for i in range(len(Pflat[:,0])): + t[i] = x[i, 0]*1E-9 + P[i,:,:] = Pflat[i,1:(numStates*numStates+1)].reshape([numStates,numStates]) + + + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.subplot(321) + plt.plot(t , x[:, 1], "b", label='Error Filter') + plt.plot(t , x[:, 1] + 3 * np.sqrt(P[:, 0, 0]), 'r--', label='Covar Filter') + plt.plot(t , x[:, 1] - 3 * np.sqrt(P[:, 0, 0]), 'r--') + plt.legend(loc='lower right') + plt.title('First pos component (m)') + plt.grid() + + plt.subplot(322) + plt.plot(t , x[:, 4], "b") + plt.plot(t , x[:, 4]+3 * np.sqrt(P[:, 3, 3]), 'r--') + plt.plot(t , x[:, 4]-3 * np.sqrt(P[:, 3, 3]), 'r--') + plt.title('Second rate component (m/s)') + plt.grid() + + plt.subplot(323) + plt.plot(t , x[:, 2], "b") + plt.plot(t , x[:, 2]+3 * np.sqrt(P[:, 1, 1]), 'r--') + plt.plot(t , x[:, 2]-3 * np.sqrt(P[:, 1, 1]), 'r--') + plt.title('Second pos component (m)') + plt.grid() + + plt.subplot(324) + plt.plot(t , x[:, 5], "b") + plt.plot(t , x[:, 5]+3 * np.sqrt(P[:, 4, 4]), 'r--') + plt.plot(t , x[:, 5]-3 * np.sqrt(P[:, 4, 4]), 'r--') + plt.xlabel('t(s)') + plt.title('Third rate component (m/s)') + plt.grid() + + plt.subplot(325) + plt.plot(t , x[:, 3], "b") + plt.plot(t , x[:, 3]+3 * np.sqrt(P[:, 2, 2]), 'r--') + plt.plot(t , x[:, 3]-3 * np.sqrt(P[:, 2, 2]), 'r--') + plt.xlabel('t(s)') + plt.title('Third pos component (m)') + plt.grid() + + plt.subplot(326) + plt.plot(t , x[:, 6], "b") + plt.plot(t , x[:, 6]+3 * np.sqrt(P[:, 5, 5]), 'r--') + plt.plot(t , x[:, 6]-3 * np.sqrt(P[:, 5, 5]), 'r--') + plt.xlabel('t(s)') + plt.title('Third rate component (m/s)') + plt.grid() + + if show_plots: + plt.show() + plt.close() + + + +def PostFitResiduals(Res, noise, testName, show_plots): + + MeasNoise = np.zeros(len(Res[:,0])) + t= np.zeros(len(Res[:,0])) + for i in range(len(Res[:,0])): + t[i] = Res[i, 0]*1E-9 + MeasNoise[i] = 3*noise + # Don't plot zero values, since they mean that no measurement is taken + for j in range(len(Res[0,:])-1): + if -1E-10 < Res[i,j+1] < 1E-10: + Res[i, j+1] = np.nan + + plt.figure(num=None, figsize=(10, 10), dpi=80, facecolor='w', edgecolor='k') + plt.subplot(311) + plt.plot(t , Res[:, 1], "b.", label='Residual') + plt.plot(t , MeasNoise, 'r--', label='Covar') + plt.plot(t , -MeasNoise, 'r--') + plt.legend(loc='lower right') + plt.ylim([-10*noise, 10*noise]) + plt.title('First Meas Comp (m)') + plt.grid() + + plt.subplot(312) + plt.plot(t , Res[:, 2], "b.") + plt.plot(t , MeasNoise, 'r--') + plt.plot(t , -MeasNoise, 'r--') + plt.ylim([-10*noise, 10*noise]) + plt.title('Second Meas Comp (m)') + plt.grid() + + plt.subplot(313) + plt.plot(t , Res[:, 3], "b.") + plt.plot(t , MeasNoise, 'r--') + plt.plot(t , -MeasNoise, 'r--') + plt.ylim([-10*noise, 10*noise]) + plt.title('Third Meas Comp (m)') + plt.grid() + + if show_plots: + plt.show() + plt.close() + +def plot_TwoOrbits(r_BN, r_BN2, show_plots): + fig = plt.figure() + ax = fig.add_subplot(projection='3d') + ax.set_xlabel('$R_x$, km') + ax.set_ylabel('$R_y$, km') + ax.set_zlabel('$R_z$, km') + ax.plot(r_BN[:, 1] * m2km, r_BN[:, 2] * m2km, r_BN[:, 3] * m2km, color_x, label="True orbit") + for i in range(len(r_BN2[:,0])): + if np.abs(r_BN2[i, 1])>0 or np.abs(r_BN2[i, 2])>0: + ax.scatter(r_BN2[i, 1] * m2km, r_BN2[i, 2] * m2km, r_BN2[i, 3] * m2km, color=color_y, label="Meas orbit") + ax.scatter(0, 0, color='r') + ax.set_title('Spacecraft Orbits') + if show_plots: + plt.show() + plt.close() + return diff --git a/src/fswAlgorithms/opticalNavigation/positionODuKF/_UnitTest/test_positionODuKF.py b/src/fswAlgorithms/opticalNavigation/positionODuKF/_UnitTest/test_positionODuKF.py new file mode 100644 index 0000000000..44328e7bf6 --- /dev/null +++ b/src/fswAlgorithms/opticalNavigation/positionODuKF/_UnitTest/test_positionODuKF.py @@ -0,0 +1,288 @@ + +# ISC License +# +# Copyright (c) 2023, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder +# +# Permission to use, copy, modify, and/or distribute this software for any +# purpose with or without fee is hereby granted, provided that the above +# copyright notice and this permission notice appear in all copies. +# +# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES +# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF +# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR +# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES +# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN +# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF +# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + + +import positionODuKF_test_utilities as FilterPlots +import numpy as np +import pytest +from Basilisk.architecture import messaging +from Basilisk.fswAlgorithms import positionODuKF # import the module that is to be tested +from Basilisk.utilities import SimulationBaseClass, macros, orbitalMotion + + +def addTimeColumn(time, data): + return np.transpose(np.vstack([[time], np.transpose(data)])) + +def rk4(f, t, x0, arg = None): + if arg is not None: + functionArg = arg + x = np.zeros([len(t),len(x0)+1]) + h = (t[len(t)-1] - t[0])/len(t) + x[0,0] = t[0] + x[0,1:] = x0 + for i in range(len(t)-1): + h = t[i+1] - t[i] + x[i,0] = t[i] + k1 = h * f(t[i], x[i,1:], functionArg) + k2 = h * f(t[i] + 0.5 * h, x[i,1:] + 0.5 * k1, functionArg) + k3 = h * f(t[i] + 0.5 * h, x[i,1:] + 0.5 * k2, functionArg) + k4 = h * f(t[i] + h, x[i,1:] + k3, functionArg) + x[i+1,1:] = x[i,1:] + (k1 + 2.*k2 + 2.*k3 + k4) / 6. + x[i+1,0] = t[i+1] + return x + +def twoBodyGrav(t, x, mu = 42828.314*1E9): + dxdt = np.zeros(np.shape(x)) + dxdt[0:3] = x[3:] + dxdt[3:] = -mu/np.linalg.norm(x[0:3])**3.*x[0:3] + return dxdt + + +def setupFilterData(filterObject): + + filterObject.alpha = 0.02 + filterObject.beta = 2.0 + + filterObject.muCentral = 42828.314*1E9 + elementsInit = orbitalMotion.ClassicElements() + elementsInit.a = 4000*1E3 #m + elementsInit.e = 0.2 + elementsInit.i = 10 + elementsInit.Omega = 0.001 + elementsInit.omega = 0.01 + elementsInit.f = 0.1 + r, v = orbitalMotion.elem2rv(filterObject.muCentral, elementsInit) + states = r.tolist() + v.tolist() + + filterObject.stateInitial = [[s] for s in states] + filterObject.covarInitial = [[1000.*1E6, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 1000.*1E6, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 1000.*1E6, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.1*1E6, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.1*1E6, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.1*1E6]] + + sigmaPos = (1E2)**2 + sigmaVel = (1E0)**2 + filterObject.processNoise = [[sigmaPos, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, sigmaPos, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, sigmaPos, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, sigmaVel, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, sigmaVel, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, sigmaVel]] + filterObject.measNoiseScaling = 100*100 + + +# uncomment this line is this test is to be skipped in the global unit test run, adjust message as needed +# @pytest.mark.skipif(conditionstring) +# uncomment this line if this test has an expected failure, adjust message as needed +# @pytest.mark.xfail() # need to update how the RW states are defined +# provide a unique test method name, starting with test_ + +def test_propagation_kf(show_plots): + """State and covariance propagation test""" + statePropPositionOD(show_plots, 10.0) +def test_measurements_kf(show_plots): + """Measurement model test""" + stateUpdatePositionOD(show_plots) + +def stateUpdatePositionOD(show_plots): + __tracebackhide__ = True + + unitTaskName = "unitTask" + unitProcessName = "TestProcess" + + # Create a sim module as an empty container + unitTestSim = SimulationBaseClass.SimBaseClass() + + # Create test thread + dt = 1.0 + t1 = 250 + multT1 = 20 + measNoiseSD = 100 # m + + testProcessRate = macros.sec2nano(dt) # update process rate update time + testProc = unitTestSim.CreateNewProcess(unitProcessName) + testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) + + # Construct algorithm and associated C++ container + moduleConfig = positionODuKF.PositionODuKF() + + # Add test module to runtime call list + unitTestSim.AddModelToTask(unitTaskName, moduleConfig) + setupFilterData(moduleConfig) + moduleConfig.measNoiseSD = measNoiseSD + + time = np.linspace(0, int(multT1*t1), int(multT1*t1//dt)+1) + energy = np.zeros(len(time)) + expected=np.zeros([len(time), 7]) + expected[0,1:] = np.array(moduleConfig.stateInitial).reshape([6,]) + energy[0] = -moduleConfig.muCentral/(2*orbitalMotion.rv2elem(moduleConfig.muCentral, expected[0,1:4], expected[0,4:]).a) + + kick = np.array([0., 0., 0., -0.01, 0.01, 0.02]) * 10 * 1E3 + + expected[0:t1,:] = rk4(twoBodyGrav, time[0:t1], expected[0,1:], arg = moduleConfig.muCentral) + expected[t1:multT1*t1+1, :] = rk4(twoBodyGrav, time[t1:len(time)], expected[t1-1, 1:] + kick, arg = moduleConfig.muCentral) + for i in range(1, len(time)): + energy[i] = - moduleConfig.muCentral / (2 * orbitalMotion.rv2elem(moduleConfig.muCentral, expected[i, 1:4], expected[i, 4:]).a) + + inputData = messaging.CameraLocalizationMsgPayload() + cameraPosMsg = messaging.CameraLocalizationMsg() + moduleConfig.cameraPosMsg.subscribeTo(cameraPosMsg) + inputData.cameraPos_N = expected[0, 1:4] + + dataLog = moduleConfig.opNavFilterMsg.recorder() + unitTestSim.AddModelToTask(unitTaskName, dataLog) + unitTestSim.InitializeSimulation() + for i in range(t1): + if i > 0 and i % 10 == 0: + inputData.timeTag = macros.sec2nano(i * dt) + inputData.cameraPos_N = (expected[i,1:4] + np.random.normal(0, measNoiseSD, 3)) + inputData.valid = True + cameraPosMsg.write(inputData, unitTestSim.TotalSim.CurrentNanos) + unitTestSim.ConfigureStopTime(macros.sec2nano((i + 1) * dt)) + unitTestSim.ExecuteSimulation() + + covarLog = addTimeColumn(dataLog.times(), dataLog.covar) + + # Propgate after the kick + for i in range(t1, 2*t1): + unitTestSim.ConfigureStopTime(macros.sec2nano((i + 1)*dt)) + unitTestSim.ExecuteSimulation() + + # Start ingesting measurements again after the kick + for i in range(t1*2, multT1*t1): + if i % 50 == 0: + inputData.timeTag = macros.sec2nano(i * dt) + inputData.cameraPos_N = (expected[i,1:4] + np.random.normal(0, measNoiseSD, 3)) + inputData.valid = True + cameraPosMsg.write(inputData, unitTestSim.TotalSim.CurrentNanos) + unitTestSim.ConfigureStopTime(macros.sec2nano((i + 1)*dt)) + unitTestSim.ExecuteSimulation() + + stateLog = addTimeColumn(dataLog.times(), dataLog.state) + postFitLog = addTimeColumn(dataLog.times(), dataLog.postFitRes) + covarLog = addTimeColumn(dataLog.times(), dataLog.covar) + + expected[:,0] *= 1E9 + diff = np.copy(stateLog) + diff[:,1:] -= expected[:,1:] + diffRel = np.copy(diff) + diffRel[:,1:] /= expected[:,1:] + FilterPlots.EnergyPlot(time, energy, 'Update', show_plots) + FilterPlots.StateCovarPlot(stateLog, covarLog, 'Update', show_plots) + FilterPlots.StatePlot(diff, 'Update', show_plots) + FilterPlots.plot_TwoOrbits(expected[:,0:4], stateLog[:,0:4], show_plots) + FilterPlots.PostFitResiduals(postFitLog, measNoiseSD, 'Update', show_plots) + + np.testing.assert_array_less(np.linalg.norm(covarLog[t1, 1:]), + np.linalg.norm(covarLog[0, 1:]), + err_msg=('Covariance must decrease during first measurement arc'), + verbose=True) + + np.testing.assert_allclose(np.linalg.norm(stateLog[:,1:4]), + np.linalg.norm(expected[:,1:4]), + rtol = 0.01, + err_msg=('Position output must match expected values'), + verbose=True) + np.testing.assert_allclose(np.linalg.norm(stateLog[:,4:]), + np.linalg.norm(expected[:,4:]), + rtol = 0.01, + err_msg=('Velocity output must match expected values'), + verbose=True) + np.testing.assert_array_less(np.linalg.norm(covarLog[t1*multT1, 1:]), + np.linalg.norm(covarLog[0, 1:])/10, + err_msg=('Covariance must decrease during second measurement arc'), + verbose=True) + return + + +def statePropPositionOD(show_plots, dt): + __tracebackhide__ = True + + unitTaskName = "unitTask" # arbitrary name (don't change) + unitProcessName = "TestProcess" # arbitrary name (don't change) + + # Create a sim module as an empty container + unitTestSim = SimulationBaseClass.SimBaseClass() + + # Create test thread + testProcessRate = macros.sec2nano(dt) # update process rate update time + testProc = unitTestSim.CreateNewProcess(unitProcessName) + testProc.addTask(unitTestSim.CreateNewTask(unitTaskName, testProcessRate)) + + # Construct algorithm and associated C++ container + moduleConfig = positionODuKF.PositionODuKF() + + # Add test module to runtime call list + unitTestSim.AddModelToTask(unitTaskName, moduleConfig) + + setupFilterData(moduleConfig) + moduleConfig.measNoiseScaling = 1 + + dataLog = moduleConfig.opNavFilterMsg.recorder() + unitTestSim.AddModelToTask(unitTaskName, dataLog) + + cameraPosMsg = messaging.CameraLocalizationMsg() + moduleConfig.cameraPosMsg.subscribeTo(cameraPosMsg) + + timeSim = 60 + time = np.linspace(0, int(timeSim*60), int(timeSim*60//dt)+1) + dydt = np.zeros(6) + energy = np.zeros(len(time)) + expected = np.zeros([len(time), 7]) + expected[0,1:] = np.array(moduleConfig.stateInitial).reshape([6,]) + energy[0] = -moduleConfig.muCentral/(2*orbitalMotion.rv2elem(moduleConfig.muCentral, expected[0,1:4], expected[0,4:]).a) + expected = rk4(twoBodyGrav, time, expected[0,1:], arg = moduleConfig.muCentral) + for i in range(1, len(time)): + energy[i] = - moduleConfig.muCentral / (2 * orbitalMotion.rv2elem(moduleConfig.muCentral, expected[i, 1:4], expected[i, 4:]).a) + + unitTestSim.InitializeSimulation() + unitTestSim.ConfigureStopTime(macros.min2nano(timeSim)) + unitTestSim.ExecuteSimulation() + + stateLog = addTimeColumn(dataLog.times(), dataLog.state) + covarLog = addTimeColumn(dataLog.times(), dataLog.covar) + + expected[:,0] *= 1E9 + diff = np.copy(stateLog) + diff[:,1:] -= expected[:,1:] + FilterPlots.plot_TwoOrbits(expected[:,0:4], stateLog[:,0:4], show_plots) + FilterPlots.EnergyPlot(time, energy, 'Prop', show_plots) + FilterPlots.StateCovarPlot(stateLog, covarLog, 'Prop', show_plots) + FilterPlots.StatePlot(diff, 'Prop', show_plots) + + np.testing.assert_allclose(stateLog, + expected, + atol = 1E-10, + err_msg=('State error'), + verbose=True) + np.testing.assert_array_less(np.linalg.norm(5*covarLog[0, 1:]), + np.linalg.norm(covarLog[-1, 1:]), + err_msg=('Covariance doesn\'t increase'), + verbose=True) + np.testing.assert_allclose(energy, + energy[0], + rtol = 1E-5, + err_msg=('Energy doesn\'t conserve'), + verbose=True) + return + + +if __name__ == "__main__": + stateUpdatePositionOD(True) From 7aab244cbc717a4c882157c859b9b4caae15f849 Mon Sep 17 00:00:00 2001 From: Thibaud Teil Date: Sat, 24 Jun 2023 14:16:41 -0600 Subject: [PATCH 3/3] Add RST documentation --- .../positionODuKF/positionODuKF.rst | 115 ++++++++++++++++++ 1 file changed, 115 insertions(+) create mode 100644 src/fswAlgorithms/opticalNavigation/positionODuKF/positionODuKF.rst diff --git a/src/fswAlgorithms/opticalNavigation/positionODuKF/positionODuKF.rst b/src/fswAlgorithms/opticalNavigation/positionODuKF/positionODuKF.rst new file mode 100644 index 0000000000..63fda6847d --- /dev/null +++ b/src/fswAlgorithms/opticalNavigation/positionODuKF/positionODuKF.rst @@ -0,0 +1,115 @@ +Executive Summary +----------------- + +This module filters position measurements that have been processed from planet images in order to +estimate spacecraft relative position to an observed body in the inertial frame. +The filter used is a square root unscented Kalman filter, and the images are first processed by image processing +and a measurement model in order to produce this filter's measurements. + +The module +:download:`PDF Description ` +contains further information on this module's mathematical framework. + +Message Connection Descriptions +------------------------------- +The following table lists all the module input and output messages. The module msg connection is set by the +user from python. The msg type contains a link to the message structure definition, while the description +provides information on what this message is used for. + +.. list-table:: Module I/O Messages + :widths: 25 25 50 + :header-rows: 1 + + * - Msg Variable Name + - Msg Type + - Description + * - navTransOutMsg + - :ref:`NavTransMsgPayload` + - navigation translation output message + * - opNavFilterMsg + - :ref:`OpNavSUKFMsgPayload` + - output filter data message containing states and covariances + * - cameraPosMsg + - :ref:`CameraLocalizationMsgPayload.h` + - opnav input message containing the position vector towards the target + +Module models +------------------------------- +The measurement model for the filter is functionally contained in the cameraTriangulation +module. The message read in therefore contains the predicted measurement: + +.. math:: + + H[X] = X[0:3] = \mathbf{r} + +The dynamics modules used are point mass, single body gravity, propagated with and RK4 +integrator + +.. math:: + + F[X] = \dot{X} = - \frac{\mu}{| X[0:3] |^3}X[0:3] = - \frac{\mu}{r^3}\mathbf{r} + +where :math:`r` is the position of the spacecraft center of masss relative to the central body +of gravitational parameter :math:`\mu`. + + +Module assumptions and limitations +------------------------------- + +The module inherits all assumptions made while implementing a Kalman filter: + • Observability considerations + • Gaussian covariances + • Linearity limits + • and more + +Otherwise, the limitations are governed by the measurement model and dynamics models relative +to the required performance. + +User Guide +---------- +This section is to outline the steps needed to setup a positionODSRuKF converter in Python. + +#. Import the module:: + + from Basilisk.fswAlgorithms import positionODSRuKF + +#. Create an instantiation of converter class:: + + positionOD = positionODuKF.PositionODuKF() + +#. Setup SRuKF general parameters:: + + positionOD.alpha = 0.02 + positionOD.beta = 2.0 + +#. Setup SRuKF measurement parameters, measurement noise Standard Deviation is given in meters:: + + positionOD.muCentral = 3000*1E9 + positionOD.measNoiseScaling = 1 + positionOD.measNoiseSD = 100 #m + +#. Setup initial state and covariances:: + + positionOD.stateInitial = [[1000.*1e3], [0], [0], [0.], [-1.*1e3], [0.]] + positionOD.covarInitial =[ [10., 0., 0., 0., 0., 0.], + [0., 10., 0., 0., 0., 0.], + [0., 0., 10., 0., 0., 0.], + [0., 0., 0., 0.01, 0., 0.], + [0., 0., 0., 0., 0.01, 0.], + [0., 0., 0., 0., 0., 0.01]] + +#. Setup process noise with units of meters and meters/s:: + + sigmaPos = 1E2 + sigmaVel = 1 + positionOD.processNoise = [[sigmaPos, 0., 0., 0., 0., 0.], + [0., sigmaPos, 0., 0., 0., 0.], + [0., 0., sigmaPos, 0., 0., 0.], + [0., 0., 0., sigmaVel, 0., 0.], + [0., 0., 0., 0., sigmaVel, 0.], + [0., 0., 0., 0., 0., sigmaVel]] + +#. Subscribe to the messages, primarily the measurement message:: + + positionOD.opNavHeadingMsg.subscribeTo(cameraTriangulation.cameraLocalizationOutMsg) +