前提・実現したいこと
Kalman filterのプログラム(MATLAB)コードの完成
ここに質問の内容を詳しく書いてください。
Githubから似たものを探して自分で変えていきましたがうまくできません
(例)PHP(CakePHP)で●●なシステムを作っています。
■■な機能を実装中に以下のエラーメッセージが発生しました。
発生している問題・エラーメッセージ
プログラムがうまくまわらず、グラフも表示されない
エラーメッセージ
該当のソースコード
ソースコード
%Define System
X = 0;
T=t; %t from measurement data
dt =0.01;
u = [v(1,:);v(2,:)]; %v from measurement data vx=v(1,:),vy=v(2,:)
n = randn();
V = randn();
%Kalman Filter variables
xt=0;
yt=0;
x = [xt:yt] %state vector
A = [1 0;0 1]; %state transition matrix
B = [dt 0;0 dt]; %control input matrix
P = 100; %std_devestd_dev = 1010
Q = 102; %process noise covariance matrix
R = 0.32; %measurement noise covariance matrix
H = [1 0;0 1];
%SN:y direction from GPS longitude
%EW:x direction from GPS latitude
%SN and EW are measurement data
% Storing calculated values in these vectors for plotting
XX = zeros(1,length(T));
tt = zeros(1,length(T));
xx_predicted = zeros(1,length(T));
xx = zeros(1,length(T));
PP = zeros(1,length(T));
yy = zeros(1,length(T));
for t=1:dt:T
%simulating the System
n = sqrt(Q) * randn(); %random noise
X = X + udt + n; % New Current State
V = sqrt(R) * randn() ;
y = HX + V; % Measurements
%Prediction Step
x_predicted = Ax + Bu; %predicting the new state (mean)
P = A * P * A' + Q; %predicting the new uncertainity (covariance)
%Correction Step
e = Hx_predicted; %expectation: predicted measurement from the o/p
E = HPH'; %Covariance of ^ expectation
z = y - e; %innovation: diff between the expectation and real sensor measurement
Z = R + E; %Covariance of ^ - sum of uncertainities of expectation and real measurement
K = PH' * Z^-1;
x = x_predicted + K*z; %final corrected state P = P - K * H* P; %uncertainity in corrected state
%Saving the outputs
xx_predicted(t) = x_predicted;
xx(t) = x;
PP(t) = P;
XX(t) = X;
tt(t) = t;
yy(t) = y;
end
plot(tt,XX,tt,xx,tt,xx_predicted,tt,yy)
hold on
試したこと
1.観測地の緯度経度情報をどこに書くのか。(緯度経度情報は既に変換している)
2.グラフでX軸Y軸が共に距離(m)で理論値、観測地(緯度経度から)、速度からの位置、カルマンフィルタの結果を出力したいです
ここに問題に対して試したことを記載してください。
補足情報(FW/ツールのバージョンなど)
ここにより詳細な情報を記載してください。
あなたの回答
tips
プレビュー