//Kalman Filter Basics
#include <iostream>
#include <math.h>
using namespace std;
double f(double mu, double sigma2, double x)
{
	//use mu, sigma2 code 1xdimensional gaussian
	double prob = 1.0/sqrt(2.0 * M_PI * sigma2) * exp(-0.5 * pow(x - mu), 2.0) / sigma2);
	return prob;
}
int main()
{
	cout <<f(10.0, 4.0, 8.0) << endl;
	return 0;
}

//1D Kalman Filter state prediction
#include <iostream>
#include <math.h>
#include <tuple>

using namespace std;

double new_mean, new_var;

tuple<double, double> measurement_update(double mean1, double var1, double mean2, double var2)
{
    new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2);
    new_var = 1 / (1 / var1 + 1 / var2);
    return make_tuple(new_mean, new_var);
}

tuple<double, double> state_prediction(double mean1, double var1, double mean2, double var2)
{
    new_mean = mean1 + mean2;
    new_var = var1 + var2;
    return make_tuple(new_mean, new_var);
}

int main()
{
    //Measurements and measurement variance
    double measurements[5] = { 5, 6, 7, 9, 10 };
    double measurement_sig = 4;
    
    //Motions and motion variance
    double motion[5] = { 1, 1, 2, 1, 1 };
    double motion_sig = 2;
    
    //Initial state
    double mu = 0;
    double sig = 1000;

    for (int i = 0; i < sizeof(measurements) / sizeof(measurements[0]); i++) {
        tie(mu, sig) = measurement_update(mu, sig, measurements[i], measurement_sig);
        printf("update:  [%f, %f]\n", mu, sig);
        tie(mu, sig) = state_prediction(mu, sig, motion[i], motion_sig);
        printf("predict: [%f, %f]\n", mu, sig);
    }

    return 0;
}

#include <iostream>
#include <math.h>
#include <tuple>
#include "Core" // Eigen Library
#include "LU"   // Eigen Library

using namespace std;
using namespace Eigen;

float measurements[3] = { 1, 2, 3 };

tuple<MatrixXf, MatrixXf> kalman_filter(MatrixXf x, MatrixXf P, MatrixXf u, MatrixXf F, MatrixXf H, MatrixXf R, MatrixXf I)
{
    for (int n = 0; n < sizeof(measurements) / sizeof(measurements[0]); n++) {

        // Measurement Update
        MatrixXf Z(1, 1);
        Z << measurements[n];

        MatrixXf y(1, 1);
        y << Z - (H * x);

        MatrixXf S(1, 1);
        S << H * P * H.transpose() + R;

        MatrixXf K(2, 1);
        K << P * H.transpose() * S.inverse();

        x << x + (K * y);

        P << (I - (K * H)) * P;

        // Prediction
        x << (F * x) + u;
        P << F * P * F.transpose();
    }

    return make_tuple(x, P);
}

int main()
{

    MatrixXf x(2, 1);// Initial state (location and velocity) 
    x << 0,
    	 0; 
    MatrixXf P(2, 2);//Initial Uncertainty
    P << 100, 0, 
    	 0, 100; 
    MatrixXf u(2, 1);// External Motion
    u << 0,
    	 0; 
    MatrixXf F(2, 2);//Next State Function
    F << 1, 1,
    	 0, 1; 
    MatrixXf H(1, 2);//Measurement Function
    H << 1,
    	 0; 
    MatrixXf R(1, 1); //Measurement Uncertainty
    R << 1;
    MatrixXf I(2, 2);// Identity Matrix
    I << 1, 0,
    	 0, 1; 

    tie(x, P) = kalman_filter(x, P, u, F, H, R, I);
    cout << "x= " << x << endl;
    cout << "P= " << P << endl;

    return 0;
}
```

```cpp
vector< vector<double> > l(mapWidth/gridWidth, vector<double>(mapHeight/gridHeight)); //creates a 2D grip each square stores a value whether it's free, occupied or unknown; ector<double>(mapHeight/gridHeight) - This constructs an unnamed inner vector to use as the second constructor parameter. The size mapHeight/gridHeight determines how many double elements will be in each inner vector
void occupancyGridMapping(double Robotx, double Roboty, double Robottheta, double sensorData[])
for (int x = 0; x < mapWidth / gridWidth; x++) {
    for (int y = 0; y < mapHeight / gridHeight; y++) {
        double xi = x * gridWidth + gridWidth / 2 - robotXOffset;
        double yi = -(y * gridHeight + gridHeight / 2) + robotYOffset;
        if (sqrt(pow(xi - Robotx, 2) + pow(yi - Roboty, 2)) <= Zmax) {
            l[x][y] = l[x][y] + inverseSensorModel(Robotx, Roboty, Robottheta, xi, yi, sensorData) - l0;
        }
    }
} //The `robotXOffset` and `robotYOffset` help to locate the robot's initial position within the grid. Here it's set to one fifth of the map's width for `robotXOffset`, and one third of the map's height for `robotYOffset`.
while (fscanf(posesFile, "%lf %lf %lf %lf", &timeStamp, &robotX, &robotY, &robotTheta) != EOF) {
    fscanf(measurementFile, "%lf", &timeStamp);
    for (int i = 0; i < 8; i++) {
        fscanf(measurementFile, "%lf", &measurementData[i]);
    }
    occupancyGridMapping(robotX, robotY, (robotTheta / 10) * (M_PI / 180), measurementData);
}
for (int x = 0; x < mapWidth / gridWidth; x++) {
    for (int y = 0; y < mapHeight / gridHeight; y++) {
        cout << l[x][y] << " ";
    }
}