Skip to content

Commit

Permalink
Feature/update readme (#8)
Browse files Browse the repository at this point in the history 
* resolve compiler warnings and add vscode settings
  • Loading branch information
@Al-khwarizmi-780
Al-khwarizmi-780 authored Apr 12, 2024
1 parent df3f221 commit 22f88dc
Show file tree
Hide file tree
Showing 12 changed files with 82 additions and 65 deletions.
17 changes: 17 additions & 0 deletions .vscode/c_cpp_properties.json
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
{
"configurations": [
{
"name": "Linux",
"includePath": [
"${workspaceFolder}/**"
],
"defines": [],
"compilerPath": "/usr/bin/clang-14",
"compileCommands": "${workspaceFolder}/build/compile_commands.json",
"cStandard": "c17",
"cppStandard": "c++14",
"intelliSenseMode": "linux-clang-x64"
}
],
"version": 4
}
2 changes: 1 addition & 1 deletion src/examples/ekf_range_sensor/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ kf::Vector<DIM_Z> covertCartesian2Polar(const kf::Vector<DIM_X> & cartesian);
kf::Matrix<DIM_Z, DIM_Z> calculateJacobianMatrix(const kf::Vector<DIM_X> & vecX);
void executeCorrectionStep();

int main(int argc, char ** argv)
int main()
{
executeCorrectionStep();

Expand Down
2 changes: 1 addition & 1 deletion src/examples/kf_state_estimation/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ static kf::KalmanFilter<DIM_X, DIM_Z> kalmanfilter;
void executePredictionStep();
void executeCorrectionStep();

int main(int argc, char ** argv)
int main()
{
executePredictionStep();
executeCorrectionStep();
Expand Down
2 changes: 1 addition & 1 deletion src/examples/sr_ukf_linear_function/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ kf::Vector<DIM_X> funcF(const kf::Vector<DIM_X> & x)
return x;
}

int main(int argc, char ** argv)
int main()
{
// example 1
runExample1();
Expand Down
2 changes: 1 addition & 1 deletion src/examples/test_least_squares/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ void runExample2();
void runExample3();
void runExample4();

int main(int argc, char ** argv)
int main()
{
runExample1();
runExample2();
Expand Down
2 changes: 1 addition & 1 deletion src/examples/ukf_range_sensor/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ kf::Vector<DIM_Z> funcH(const kf::Vector<DIM_X> & x, const kf::Vector<DIM_N> & n
return y;
}

int main(int argc, char ** argv)
int main()
{
// example 1
runExample1();
Expand Down
2 changes: 1 addition & 1 deletion src/examples/unscented_transform/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ kf::Vector<DIM_2> function2(const kf::Vector<DIM_2> & x)
return y;
}

int main(int argc, char ** argv)
int main()
{
// example 1
runExample1();
Expand Down
16 changes: 8 additions & 8 deletions src/openkf/kalman_filter/square_root_ukf.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,7 +107,7 @@ namespace kf
// y_sigmas = np.zeros((self.dim_x, self.n_sigma))
// for i in range(self.n_sigma):
// y_sigmas[:, i] = f(x_sigmas[:, i])
for (size_t i{ 0 }; i < SIGMA_DIM; ++i)
for (int32_t i{ 0 }; i < SIGMA_DIM; ++i)
{
const Vector<DIM_X> Xi{ util::getColumnAt<DIM_X, SIGMA_DIM>(i, matSigmaX) };
const Vector<DIM_X> Yi{ predictionModelFunc(Xi) }; // y = f(x)
Expand Down Expand Up @@ -140,7 +140,7 @@ namespace kf
// y_sigmas[:, i] = f(x_sigmas[:, i])
Matrix<DIM_Z, SIGMA_DIM> matSigmaY;

for (size_t i{ 0 }; i < SIGMA_DIM; ++i)
for (int32_t i{ 0 }; i < SIGMA_DIM; ++i)
{
const Vector<DIM_X> Xi{ util::getColumnAt<DIM_X, SIGMA_DIM>(i, matSigmaX) };
const Vector<DIM_Z> Yi{ measurementModelFunc(Xi) }; // y = f(x)
Expand Down Expand Up @@ -217,10 +217,10 @@ namespace kf
// X_0 = \bar{xa}
util::copyToColumn< DIM_X, SIGMA_DIM >(0, sigmaX, vecXk);

for (size_t i{ 0 }; i < DIM_X; ++i)
for (int32_t i{ 0 }; i < DIM_X; ++i)
{
const size_t IDX_1{ i + 1 };
const size_t IDX_2{ i + DIM_X + 1 };
const int32_t IDX_1{ i + 1 };
const int32_t IDX_2{ i + DIM_X + 1 };

util::copyToColumn< DIM_X, SIGMA_DIM >(IDX_1, sigmaX, vecXk);
util::copyToColumn< DIM_X, SIGMA_DIM >(IDX_2, sigmaX, vecXk);
Expand All @@ -239,12 +239,12 @@ namespace kf
/// @param sigmaX matrix of sigma points where each column contain single sigma point
/// @param vecX output weighted mean
///
template<size_t DIM>
template<int32_t DIM>
void calculateWeightedMean(const Matrix<DIM, SIGMA_DIM> & sigmaX, Vector<DIM> & vecX)
{
// 1. calculate mean: \bar{y} = \sum_{i_0}^{2n} W[0, i] Y[:, i]
vecX = m_weight0 * util::getColumnAt<DIM, SIGMA_DIM>(0, sigmaX);
for (size_t i{ 1 }; i < SIGMA_DIM; ++i)
for (int32_t i{ 1 }; i < SIGMA_DIM; ++i)
{
vecX += m_weighti * util::getColumnAt<DIM, SIGMA_DIM>(i, sigmaX); // y += W[0, i] Y[:, i]
}
Expand All @@ -270,7 +270,7 @@ namespace kf
m_weight0 * (devXi * devYi.transpose())
};

for (size_t i{ 1 }; i < SIGMA_DIM; ++i)
for (int32_t i{ 1 }; i < SIGMA_DIM; ++i)
{
devXi = util::getColumnAt<DIM_X, SIGMA_DIM>(i, sigmaX) - vecX; // X[:, i] - \bar{x}
devYi = util::getColumnAt<DIM_Z, SIGMA_DIM>(i, sigmaY) - vecY; // Y[:, i] - \bar{y}
Expand Down
38 changes: 19 additions & 19 deletions src/openkf/kalman_filter/unscented_kalman_filter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,12 +40,12 @@ namespace kf
///
void setCovarianceQ(const Matrix<DIM_V, DIM_V> & matQ)
{
const size_t S_IDX{ DIM_X };
const size_t L_IDX{ S_IDX + DIM_V };
const int32_t S_IDX{ DIM_X };
const int32_t L_IDX{ S_IDX + DIM_V };

for (size_t i{ S_IDX }; i < L_IDX; ++i)
for (int32_t i{ S_IDX }; i < L_IDX; ++i)
{
for (size_t j{ S_IDX }; j < L_IDX; ++j)
for (int32_t j{ S_IDX }; j < L_IDX; ++j)
{
m_matPa(i, j) = matQ(i - S_IDX, j - S_IDX);
}
Expand All @@ -58,12 +58,12 @@ namespace kf
///
void setCovarianceR(const Matrix<DIM_N, DIM_N> & matR)
{
const size_t S_IDX{ DIM_X + DIM_V };
const size_t L_IDX{ S_IDX + DIM_N };
const int32_t S_IDX{ DIM_X + DIM_V };
const int32_t L_IDX{ S_IDX + DIM_N };

for (size_t i{ S_IDX }; i < L_IDX; ++i)
for (int32_t i{ S_IDX }; i < L_IDX; ++i)
{
for (size_t j{ S_IDX }; j < L_IDX; ++j)
for (int32_t j{ S_IDX }; j < L_IDX; ++j)
{
m_matPa(i, j) = matR(i - S_IDX, j - S_IDX);
}
Expand Down Expand Up @@ -105,7 +105,7 @@ namespace kf
// y_sigmas = np.zeros((self.dim_x, self.n_sigma))
// for i in range(self.n_sigma):
// y_sigmas[:, i] = f(xx_sigmas[:, i], xv_sigmas[:, i])
for (size_t i{ 0 }; i < SIGMA_DIM; ++i)
for (int32_t i{ 0 }; i < SIGMA_DIM; ++i)
{
const Vector<DIM_X> sigmaXxi{ util::getColumnAt<DIM_X, SIGMA_DIM>(i, sigmaXx) };
const Vector<DIM_V> sigmaXvi{ util::getColumnAt<DIM_V, SIGMA_DIM>(i, sigmaXv) };
Expand Down Expand Up @@ -150,7 +150,7 @@ namespace kf
// for i in range(self.n_sigma) :
// y_sigmas[:, i] = h(xx_sigmas[:, i], xn_sigmas[:, i])
Matrix<DIM_Z, SIGMA_DIM> sigmaY;
for (size_t i{ 0 }; i < SIGMA_DIM; ++i)
for (int32_t i{ 0 }; i < SIGMA_DIM; ++i)
{
const Vector<DIM_X> sigmaXxi{ util::getColumnAt<DIM_X, SIGMA_DIM>(i, sigmaXx) };
const Vector<DIM_N> sigmaXni{ util::getColumnAt<DIM_N, SIGMA_DIM>(i, sigmaXn) };
Expand Down Expand Up @@ -194,7 +194,7 @@ namespace kf
///
void updateAugmentedVecX()
{
for (size_t i{ 0 }; i < DIM_X; ++i)
for (int32_t i{ 0 }; i < DIM_X; ++i)
{
m_vecXa[i] = m_vecX[i];
}
Expand All @@ -205,9 +205,9 @@ namespace kf
///
void updateAugmentedMatP()
{
for (size_t i{ 0 }; i < DIM_X; ++i)
for (int32_t i{ 0 }; i < DIM_X; ++i)
{
for (size_t j{ 0 }; j < DIM_X; ++j)
for (int32_t j{ 0 }; j < DIM_X; ++j)
{
m_matPa(i, j) = m_matP(i, j);
}
Expand Down Expand Up @@ -251,10 +251,10 @@ namespace kf
// X_0 = \bar{xa}
util::copyToColumn< DIM_A, SIGMA_DIM >(0, sigmaXa, vecXa);

for (size_t i{ 0 }; i < DIM_A; ++i)
for (int32_t i{ 0 }; i < DIM_A; ++i)
{
const size_t IDX_1{ i + 1 };
const size_t IDX_2{ i + DIM_A + 1 };
const int32_t IDX_1{ i + 1 };
const int32_t IDX_2{ i + DIM_A + 1 };

util::copyToColumn< DIM_A, SIGMA_DIM >(IDX_1, sigmaXa, vecXa);
util::copyToColumn< DIM_A, SIGMA_DIM >(IDX_2, sigmaXa, vecXa);
Expand All @@ -279,7 +279,7 @@ namespace kf
{
// 1. calculate mean: \bar{y} = \sum_{i_0}^{2n} W[0, i] Y[:, i]
vecX = m_weight0 * util::getColumnAt<STATE_DIM, SIGMA_DIM>(0, sigmaX);
for (size_t i{ 1 }; i < SIGMA_DIM; ++i)
for (int32_t i{ 1 }; i < SIGMA_DIM; ++i)
{
vecX += m_weighti * util::getColumnAt<STATE_DIM, SIGMA_DIM>(i, sigmaX); // y += W[0, i] Y[:, i]
}
Expand All @@ -288,7 +288,7 @@ namespace kf
Vector<STATE_DIM> devXi{ util::getColumnAt<STATE_DIM, SIGMA_DIM>(0, sigmaX) - vecX }; // Y[:, 0] - \bar{ y }
matPxx = m_weight0 * devXi * devXi.transpose(); // P_0 = W[0, 0] (Y[:, 0] - \bar{y}) (Y[:, 0] - \bar{y})^T

for (size_t i{ 1 }; i < SIGMA_DIM; ++i)
for (int32_t i{ 1 }; i < SIGMA_DIM; ++i)
{
devXi = util::getColumnAt<STATE_DIM, SIGMA_DIM>(i, sigmaX) - vecX; // Y[:, i] - \bar{y}

Expand Down Expand Up @@ -318,7 +318,7 @@ namespace kf
m_weight0 * (devXi * devYi.transpose())
};

for (size_t i{ 1 }; i < SIGMA_DIM; ++i)
for (int32_t i{ 1 }; i < SIGMA_DIM; ++i)
{
devXi = util::getColumnAt<DIM_X, SIGMA_DIM>(i, sigmaX) - vecX; // X[:, i] - \bar{x}
devYi = util::getColumnAt<DIM_Z, SIGMA_DIM>(i, sigmaY) - vecY; // Y[:, i] - \bar{y}
Expand Down
22 changes: 11 additions & 11 deletions src/openkf/kalman_filter/unscented_transform.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,11 +18,11 @@

namespace kf
{
template<size_t DIM>
template<int32_t DIM>
class UnscentedTransform
{
public:
static constexpr size_t SIGMA_DIM{ (2 * DIM) + 1 };
static constexpr int32_t SIGMA_DIM{ (2 * DIM) + 1 };

UnscentedTransform() {}
~UnscentedTransform() {}
Expand All @@ -45,12 +45,12 @@ namespace kf
updateSigmaPoints(vecX, matPxx, kappa);
}

template<size_t DIM_X>
template<int32_t DIM_X>
void calculateWeightedMeanAndCovariance(const Matrix<DIM_X, SIGMA_DIM> & sigmaX, Vector<DIM_X> & vecX, Matrix<DIM_X, DIM_X> & matPxx)
{
// 1. calculate mean: \bar{y} = \sum_{i_0}^{2n} W[0, i] Y[:, i]
vecX = _weights[0] * util::getColumnAt<DIM_X, SIGMA_DIM>(0, sigmaX);
for (size_t i{ 1 }; i < SIGMA_DIM; ++i)
for (int32_t i{ 1 }; i < SIGMA_DIM; ++i)
{
vecX += _weights[1] * util::getColumnAt<DIM_X, SIGMA_DIM>(i, sigmaX); // y += W[0, i] Y[:, i]
}
Expand All @@ -59,7 +59,7 @@ namespace kf
Vector<DIM_X> devXi{ util::getColumnAt<DIM_X, SIGMA_DIM>(0, sigmaX) - vecX }; // Y[:, 0] - \bar{ y }
matPxx = _weights[0] * devXi * devXi.transpose(); // P_0 = W[0, 0] (Y[:, 0] - \bar{y}) (Y[:, 0] - \bar{y})^T

for (size_t i{ 1 }; i < SIGMA_DIM; ++i)
for (int32_t i{ 1 }; i < SIGMA_DIM; ++i)
{
devXi = util::getColumnAt<DIM_X, SIGMA_DIM>(i, sigmaX) - vecX; // Y[:, i] - \bar{y}

Expand Down Expand Up @@ -145,10 +145,10 @@ namespace kf
// X_0 = \bar{x}
util::copyToColumn< DIM, SIGMA_DIM >(0, _sigmaX, vecX);

for (size_t i{ 0 }; i < DIM; ++i)
for (int32_t i{ 0 }; i < DIM; ++i)
{
const size_t IDX_1{ i + 1 };
const size_t IDX_2{ i + DIM + 1 };
const int32_t IDX_1{ i + 1 };
const int32_t IDX_2{ i + DIM + 1 };

util::copyToColumn< DIM, SIGMA_DIM >(IDX_1, _sigmaX, vecX);
util::copyToColumn< DIM, SIGMA_DIM >(IDX_2, _sigmaX, vecX);
Expand All @@ -168,7 +168,7 @@ namespace kf
template<typename NonLinearFunctionCallback>
void transformSigmaPoints(NonLinearFunctionCallback nonlinearFunction, Matrix<DIM, SIGMA_DIM> & sigmaY)
{
for (size_t i{ 0 }; i < SIGMA_DIM; ++i)
for (int32_t i{ 0 }; i < SIGMA_DIM; ++i)
{
const Vector<DIM> x{ util::getColumnAt<DIM, SIGMA_DIM>(i, _sigmaX) };
const Vector<DIM> y{ nonlinearFunction(x) }; // y = f(x)
Expand All @@ -187,7 +187,7 @@ namespace kf
{
// 1. calculate mean: \bar{y} = \sum_{i_0}^{2n} W[0, i] Y[:, i]
vecY = _weights[0] * util::getColumnAt<DIM, SIGMA_DIM>(0, sigmaY);
for (size_t i{ 1 }; i < SIGMA_DIM; ++i)
for (int32_t i{ 1 }; i < SIGMA_DIM; ++i)
{
vecY += _weights[1] * util::getColumnAt<DIM, SIGMA_DIM>(i, sigmaY); // y += W[0, i] Y[:, i]
}
Expand All @@ -196,7 +196,7 @@ namespace kf
Vector<DIM> devYi{ util::getColumnAt<DIM, SIGMA_DIM>(0, sigmaY) - vecY }; // Y[:, 0] - \bar{ y }
matPyy = _weights[0] * devYi * devYi.transpose(); // P_0 = W[0, 0] (Y[:, 0] - \bar{y}) (Y[:, 0] - \bar{y})^T

for (size_t i{ 1 }; i < SIGMA_DIM; ++i)
for (int32_t i{ 1 }; i < SIGMA_DIM; ++i)
{
devYi = util::getColumnAt<DIM, SIGMA_DIM>(i, sigmaY) - vecY; // Y[:, i] - \bar{y}

Expand Down
4 changes: 2 additions & 2 deletions src/openkf/types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,10 +19,10 @@ namespace kf
{
using float32_t = float;

template<size_t ROW, size_t COL>
template<int32_t ROW, int32_t COL>
using Matrix = Eigen::Matrix<float32_t, ROW, COL>;

template<size_t ROW>
template<int32_t ROW>
using Vector = Eigen::Matrix<float32_t, ROW, 1>;
}

Expand Down
Loading

0 comments on commit 22f88dc

Please sign in to comment.