M5SticKC_Plusから、、、ESP32ではカルマンフィルタが簡単に使えるので
倒立振子ロボでカルマンフィルタを使ってるけど
人に説明する時がちょいちょいあるのだけど
知らない人に説明するのになんか微妙になることが多い。
知ってる人からは、たいていアポロ１１号の軌道計算とかで有名なやつ・・・
って話よく出る。
最近はプロセッシングちょこちょこ使う。
アニメーションとかちょっと混ぜるのに便利。
でもって簡単なKalmanフィルタの処理のイメージを理解するのに使えそうな
ツール作ってみた。
#カルマンフィルタ、
— しん_2-41 (@shinichi_nino) 2025年6月22日
#倒立振子 ロボ 、#ｽﾀｯｸﾁｬﾝ で使ってます#LPF とかとの違いを直感的に
理解できる助け...かもしれない
ちょっとツール、#processing で作ってみた。
コード、ブログに載せてます、コピペで動きますhttps://t.co/VJutljgFcH#M5Stack https://t.co/3wh3fimhcV pic.twitter.com/qqaWpIShBD
カルマンフィルタの考え方は・・・
理論的なとこは検索したらいろいろ出ますが自分が人に説明でよく言うのは,,,
直近の過去データから、最小二乗法的に近似直線を求め、
現在値もおよそそのラインの延長上の現在点にデータは来ると予測し、
実際に観測した現在値はノイズを含んではいるが真値の情報が含まれているので、、、
そこから予測した直線上と観測値に各々、そのデータ特性を考慮して重み付けをした上で重みづけ予測値と重みづけ観測値の平均を取って現在値の真値を推定する手法。
ってな感じで説明すること多い。・・・カルマンフィルタの2次元版
近似直性で現在値、位置から方向を予測しての2次元これだと現在値を予測してって感じが直感的にもわかりやすい・・・かな。
ただ演算はけっこう重くなる。 昔のマイコンでは変化速度の遅い信号、でもってノイズが多くなってしまうセンサー値などではそれやってた。
倒立振子ロボではカルマンフィルタは使ってるけど、自分で書くと面倒なので
ライブラリがあるので使ってるけどどんな演算してるのかまで見てない...
フツーのLPFより効果があったんでよしとして使ってる。。。^^;
あんましよくないですね。
自分でCで組んでも倒立振子ロボのG,ジャイロ信号maxで3Hzくらいかなぁなら
ESP32ならちゃんと過去AD10点くらいで近似直線計算して。。。
でもいけそうだけど。
スケッチのコードはこちら・・・
説明はまた別途追記していこうかと思います。
例によって、あんましじっくり確認してないので
おかしいんじゃないってのあったらご一報ください...^^;
ただ、かなり簡易版です。
スカラー版（状態：位置のみ）のカルマンフィルタ演算なので、基本的に考え方は
「前回値（推定値）」を元に「観測値」との重み付き平均をしているだけです。
なので、、、
カルマンフィルタでよく言われる「未来を予測している」とは言いづらく、
「ノイズを和らげているだけ＝LPFに近い処理」 とみなす方が正確です。。。
実際以下のコードではあんましカルマンフィルタとLPF差がないです。。。
// カルマンフィルタ、LPFの簡易比較スケッチ
//
float trueValue = 0;
float observedValue = 0;
float kalmanEstimate = 0;
float lpfEstimate = 0;
float K = 0;
float P = 1;
float Q = 0.01;
float R = 1.0;
float freq = 1.0;
float lpfCutoff = 1.0;
float noiseStd = 5.0;
float t = 0;
ArrayList<Float> trueHistory = new ArrayList<Float>();
ArrayList<Float> observedHistory = new ArrayList<Float>();
ArrayList<Float> kalmanHistory = new ArrayList<Float>();
ArrayList<Float> lpfHistory = new ArrayList<Float>();
int sliderWidth = 300;
int sliderHeight = 20;
float sliderQX = 50;
float sliderRX = 50;
float sliderLPFX = 50;
float sliderFreqX;
float sliderNoiseX;
float sliderY = 30;
float sliderMaxQ = 0.1;
float sliderMaxR = 10.0;
float sliderMinFreq = 0.1;
float sliderMaxFreq = 5.0;
float sliderMinCutoff = 0.1;
float sliderMaxCutoff = 5.0;
float sliderMinNoise = 1.0;
float sliderMaxNoise = 20.0;
boolean draggingQ = false;
boolean draggingR = false;
boolean draggingFreq = false;
boolean draggingCutoff = false;
boolean draggingNoise = false;
boolean showTrue = true;
boolean showObserved = true;
boolean showKalman = true;
boolean showLPF = true;
int[ ][ ] legendBoxes = new int[4][4];
int baseWidth = 800, baseHeight = 640;
boolean isFullscreen = false;
void setup() {
size(800, 600);
// size(baseWidth, baseHeight);
surface.setResizable(true);
}
void draw() {
background(255);
float dt = 1.0 / frameRate;
t += dt;
// スライダ位置（右側スライダ動的に配置）
float margin = 50;
sliderFreqX = width - margin - sliderWidth;
sliderNoiseX = width - margin - sliderWidth;
// スライダ値取得
float mouseQ = map(constrain(mouseX, sliderQX, sliderQX + sliderWidth), sliderQX, sliderQX + sliderWidth, 0.0001, sliderMaxQ);
float mouseR = map(constrain(mouseX, sliderRX, sliderRX + sliderWidth), sliderRX, sliderRX + sliderWidth, 0.01, sliderMaxR);
float mouseFreq = map(constrain(mouseX, sliderFreqX, sliderFreqX + sliderWidth), sliderFreqX, sliderFreqX + sliderWidth, sliderMinFreq, sliderMaxFreq);
float mouseCutoff = map(constrain(mouseX, sliderLPFX, sliderLPFX + sliderWidth), sliderLPFX, sliderLPFX + sliderWidth, sliderMinCutoff, sliderMaxCutoff);
float mouseNoise = map(constrain(mouseX, sliderNoiseX, sliderNoiseX + sliderWidth), sliderNoiseX, sliderNoiseX + sliderWidth, sliderMinNoise, sliderMaxNoise);
if (draggingQ) Q = mouseQ;
if (draggingR) R = mouseR;
if (draggingFreq) freq = mouseFreq;
if (draggingCutoff) lpfCutoff = mouseCutoff;
if (draggingNoise) noiseStd = mouseNoise;
float alpha = (TWO_PI * lpfCutoff * dt) / (1 + TWO_PI * lpfCutoff * dt);
trueValue = sin(TWO_PI * freq * t) * 100;
observedValue = trueValue + randomGaussian() * noiseStd;
P += Q;
K = P / (P + R);
kalmanEstimate += K * (observedValue - kalmanEstimate);
P = (1 - K) * P;
lpfEstimate += alpha * (observedValue - lpfEstimate);
trueHistory.add(trueValue);
observedHistory.add(observedValue);
kalmanHistory.add(kalmanEstimate);
lpfHistory.add(lpfEstimate);
if (trueHistory.size() > width) {
trueHistory.remove(0);
observedHistory.remove(0);
kalmanHistory.remove(0);
lpfHistory.remove(0);
}
// グラフ表示
int graphTop = 240;
translate(0, graphTop + (height - graphTop) / 2);
if (showTrue) {
stroke(255, 204, 0);
noFill();
beginShape();
for (int i = 0; i < trueHistory.size(); i++) vertex(i, -trueHistory.get(i));
endShape();
}
if (showObserved) {
stroke(200, 0, 0);
noFill();
beginShape();
for (int i = 0; i < observedHistory.size(); i++) vertex(i, -observedHistory.get(i));
endShape();
}
if (showKalman) {
stroke(0, 0, 255);
noFill();
beginShape();
for (int i = 0; i < kalmanHistory.size(); i++) vertex(i, -kalmanHistory.get(i));
endShape();
}
if (showLPF) {
stroke(0, 150, 0);
noFill();
beginShape();
for (int i = 0; i < lpfHistory.size(); i++) vertex(i, -lpfHistory.get(i));
endShape();
}
resetMatrix();
// 左上にK表示
fill(0);
textAlign(LEFT);
text("Kalman_filter_gain" + nf(K, 1, 3), 100, 10);
// 左スライダ
drawSlider(sliderQX, sliderY, sliderWidth, sliderHeight, Q, sliderMaxQ, "Q_val");
drawSlider(sliderRX, sliderY + 40, sliderWidth, sliderHeight, R, sliderMaxR, "R_val");
// LPFは間隔をあけて配置
drawSlider(sliderLPFX, sliderY + 100, sliderWidth, sliderHeight, lpfCutoff, sliderMaxCutoff, "LPF_[Hz]");
// 右スライダ
drawSlider(sliderFreqX, sliderY, sliderWidth, sliderHeight, freq, sliderMaxFreq, "signal[Hz]");
drawSlider(sliderNoiseX, sliderY + 40, sliderWidth, sliderHeight, noiseStd, sliderMaxNoise, "noize(%)");
// 凡例
int legendX = 10;
int legendY = height - 25;
int spacing = 110;
drawLegend(0, legendX + spacing * 0, legendY, color(255, 204, 0), "signal", showTrue);
drawLegend(1, legendX + spacing * 1, legendY, color(200, 0, 0), "AD_val", showObserved);
drawLegend(2, legendX + spacing * 2, legendY, color(0, 0, 255), "kalman", showKalman);
drawLegend(3, legendX + spacing * 3, legendY, color(0, 150, 0), "LPF", showLPF);
}
void drawSlider(float x, float y, float w, float h, float value, float maxVal, String label) {
fill(220);
rect(x, y, w, h);
float pos = map(value, 0, maxVal, x, x + w);
fill(100, 150, 255);
rect(x, y, pos - x, h);
fill(0);
text(label + " = " + nf(value, 1, label.contains("noize") ? 1 : 3), x, y - 5);
}
void drawLegend(int index, int x, int y, color c, String label, boolean enabled) {
int boxSize = 12;
legendBoxes[index][0] = x;
legendBoxes[index][1] = y - boxSize + 4;
legendBoxes[index][2] = boxSize;
legendBoxes[index][3] = boxSize;
fill(enabled ? c : 180);
stroke(0);
rect(x, y - boxSize + 4, boxSize, boxSize);
fill(0);
textAlign(LEFT, CENTER);
text(label, x + boxSize + 5, y);
}
void toggleLegend(int index) {
if (index == 0) showTrue = !showTrue;
if (index == 1) showObserved = !showObserved;
if (index == 2) showKalman = !showKalman;
if (index == 3) showLPF = !showLPF;
}
void mousePressed() {
if (overSlider(sliderQX, sliderY)) draggingQ = true;
else if (overSlider(sliderRX, sliderY + 40)) draggingR = true;
else if (overSlider(sliderLPFX, sliderY + 100)) draggingCutoff = true;
else if (overSlider(sliderFreqX, sliderY)) draggingFreq = true;
else if (overSlider(sliderNoiseX, sliderY + 40)) draggingNoise = true;
else {
for (int i = 0; i < 4; i++) {
int x = legendBoxes[i][0];
int y = legendBoxes[i][1];
int w = legendBoxes[i][2];
int h = legendBoxes[i][3];
if (mouseX >= x && mouseX <= x + w && mouseY >= y && mouseY <= y + h) {
toggleLegend(i);
break;
}
}
}
}
void mouseReleased() {
draggingQ = false;
draggingR = false;
draggingCutoff = false;
draggingFreq = false;
draggingNoise = false;
}
boolean overSlider(float x, float y) {
return (mouseX >= x && mouseX <= x + sliderWidth &&
mouseY >= y && mouseY <= y + sliderHeight);
}
// F11キーで最大化/復元
void keyPressed() {
if (key == CODED && keyCode == java.awt.event.KeyEvent.VK_F11) {
isFullscreen = !isFullscreen;
if (isFullscreen) surface.setSize(displayWidth, displayHeight);
else surface.setSize(baseWidth, baseHeight);
}
}
