卡尔曼滤波器

#ifndef _MYKALMAN_H
#define _MYKALMAN_H
#include <Eigen\Dense>


class KalmanFilter
{
private:
	int stateSize; //state variable's dimenssion
	int measSize; //measurement variable's dimession
	int uSize; //control variables's dimenssion
	Eigen::VectorXd x;
	Eigen::VectorXd z;
	Eigen::MatrixXd A;
	Eigen::MatrixXd B;
	Eigen::VectorXd u;
	Eigen::MatrixXd P;//coveriance
	Eigen::MatrixXd H;
	Eigen::MatrixXd R;//measurement noise covariance
	Eigen::MatrixXd Q;//process noise covariance
public:
	KalmanFilter(int stateSize_, int measSize_,int uSize_);
	~KalmanFilter(){}
	void init(Eigen::VectorXd &x_, Eigen::MatrixXd& P_,Eigen::MatrixXd& R_, Eigen::MatrixXd& Q_);
	Eigen::VectorXd predict(Eigen::MatrixXd& A_);
	Eigen::VectorXd predict(Eigen::MatrixXd& A_, Eigen::MatrixXd &B_, Eigen::VectorXd &u_);
	void update(Eigen::MatrixXd& H_,Eigen::VectorXd z_meas);
};
KalmanFilter::KalmanFilter(int stateSize_ = 0, int measSize_ = 0, int uSize_=0) :stateSize(stateSize_), measSize(measSize_), uSize(uSize_)
{
	if (stateSize == 0 || measSize == 0)
	{
		std::cerr << "Error, State size and measurement size must bigger than 0\n";
	}

	x.resize(stateSize);
	x.setZero();

	A.resize(stateSize, stateSize);
	A.setIdentity();

	u.resize(uSize);
	u.transpose();
	u.setZero();

	B.resize(stateSize, uSize);
	B.setZero();

	P.resize(stateSize, stateSize);
	P.setIdentity();

	H.resize(measSize, stateSize);
	H.setZero();

	z.resize(measSize);
	z.setZero();

	Q.resize(stateSize, stateSize);
	Q.setZero();

	R.resize(measSize, measSize);
	R.setZero();
}

void KalmanFilter::init(Eigen::VectorXd &x_, Eigen::MatrixXd& P_, Eigen::MatrixXd& R_, Eigen::MatrixXd& Q_)
{
	x = x_;
	P = P_;
	R = R_;
	Q = Q_;
}
Eigen::VectorXd KalmanFilter::predict(Eigen::MatrixXd& A_, Eigen::MatrixXd &B_, Eigen::VectorXd &u_)
{
	A = A_;
	B = B_;
	u = u_;
	x = A*x + B*u;
	Eigen::MatrixXd A_T = A.transpose();
	P = A*P*A_T + Q;
	return x;
}

Eigen::VectorXd KalmanFilter::predict(Eigen::MatrixXd& A_)
{
	A = A_;
	x = A*x;
	Eigen::MatrixXd A_T = A.transpose();
	P = A*P*A_T + Q; 
//	cout << "P-=" << P<< endl;
	return x;
}

void KalmanFilter::update(Eigen::MatrixXd& H_,Eigen::VectorXd z_meas)
{
	H = H_;
	Eigen::MatrixXd temp1, temp2,Ht;
	Ht = H.transpose();
	temp1 = H*P*Ht + R;
	temp2 = temp1.inverse();//(H*P*H'+R)^(-1)
	Eigen::MatrixXd K = P*Ht*temp2;
	z = H*x;
	x = x + K*(z_meas-z);
	Eigen::MatrixXd I = Eigen::MatrixXd::Identity(stateSize, stateSize);
	P = (I - K*H)*P;
//	cout << "P=" << P << endl;
}

Julia

using LinearAlgebra

struct KalmanFilter
    stateSize::Int
    measSize::Int
    uSize::Int
    x::Vector{Float64}
    z::Vector{Float64}
    A::Matrix{Float64}
    B::Matrix{Float64}
    u::Vector{Float64}
    P::Matrix{Float64}
    H::Matrix{Float64}
    R::Matrix{Float64}
    Q::Matrix{Float64}
end

function KalmanFilter(stateSize::Int = 0, measSize::Int = 0, uSize::Int = 0)
    if stateSize == 0 || measSize == 0
        throw(ArgumentError("Error, State size and measurement size must be bigger than 0"))
    end

    x = zeros(stateSize)
    A = Matrix{Float64}(I, stateSize, stateSize)
    u = zeros(uSize)
    B = zeros(stateSize, uSize)
    P = Matrix{Float64}(I, stateSize, stateSize)
    H = zeros(measSize, stateSize)
    z = zeros(measSize)
    Q = zeros(stateSize, stateSize)
    R = zeros(measSize, measSize)

    KalmanFilter(stateSize, measSize, uSize, x, z, A, B, u, P, H, R, Q)
end

function init!(kf::KalmanFilter, x::Vector{Float64}, P::Matrix{Float64}, R::Matrix{Float64}, Q::Matrix{Float64})
    kf.x = x
    kf.P = P
    kf.R = R
    kf.Q = Q
end

function predict!(kf::KalmanFilter, A::Matrix{Float64}, B::Matrix{Float64}, u::Vector{Float64})
    kf.A = A
    kf.B = B
    kf.u = u
    kf.x .= A * kf.x .+ B * kf.u
    kf.P .= A * kf.P * A' .+ kf.Q
    return kf.x
end

function predict!(kf::KalmanFilter, A::Matrix{Float64})
    kf.A = A
    kf.x .= A * kf.x
    kf.P .= A * kf.P * A' .+ kf.Q
    return kf.x
end

function update!(kf::KalmanFilter, H::Matrix{Float64}, z_meas::Vector{Float64})
    kf.H = H
    temp1 = kf.H * kf.P * kf.H' .+ kf.R
    temp2 = inv(temp1)
    K = kf.P * kf.H' * temp2
    kf.z .= kf.H * kf.x
    kf.x .+= K * (z_meas .- kf.z)
    I = Matrix{Float64}(I, kf.stateSize, kf.stateSize)
    kf.P = (I .- K * kf.H) * kf.P
end