序文
このプロジェクトは、主に加速度計と気圧計を入力として使用し、カルマン フィルター融合後の高度、速度、加速度を出力します。これら 3 つの出力は、製品の特定のアクチュエーターのドローンの飛行特性を満たす必要があり、このプロジェクトも同様ですSimulink 上でのオリジナルのアルゴリズムの再現と最適化に基づいています。
void update_sys() {
X_hat = F * X;
P_hat = F * P * F.transpose() + Q;
P_inv = H * P_hat * H.transpose() + R;
K = P_hat * H.transpose() * inv(P_inv);
X = X_hat + K * (Y - H * X_hat);
P = (I - K * H) * P_hat;
}
アルゴリズムモデル
アルゴリズム原理
KF は実際には最適再帰的データ処理アルゴリズムです。変化するデータからノイズを除去し、システムの将来の出力を予測するアルゴリズムは、確率と統計の原理に基づいた予測アルゴリズムであり、アルゴリズムの入力は測定可能です (たとえば、私の例の入力は加速度計と気圧計を使用しています)この測定値を使用すると、測定値に基づいてシステムの実際の出力を推定することができ、同時にこの値の新しい推定精度を与えることができます。これはカルマンフィルタリングの仕事ですが、この仕事は継続的に実行され、システムは継続的に測定され、その後推定されるため、一定期間後にシステムの非常に正確な出力値を推定することができます。ここで明確にしておきたいのは、測定値は非常に不正確である可能性があり、推定値も非常に不正確である可能性があり、これはエンジニアリングにおける多くの作業条件と一致しますが、最終的にはこれら 2 つの不正確な値に基づいてのみ、比較的正確なシステム カルマン フィルターの役割となる出力値。
アルゴリズム5つの重要な公式
予測プロセス:
補正プロセス:
(この記事は Simulink カルマン フィルター システムのモデル化に関するものであるため、ここでは式の導出についてはあまり詳しく説明しません。)
Simulink モジュラー モデリング
サブシステム
SubSystem を使用して、データを計算して転送するための各式のサブシステムを確立します。この利点は、各式を単体テストできること、つまり、シミュレーション プロセス中にデータをより適切に処理できることです。
完全なモデル図
サブモジュールを使用してビルドすると、行のレイアウトが少し乱雑になるのはわかりますが、これを置き換えるために、より最適化されたソリューションである S-Function-Builder については後述します。
予測する
状態予測 (式 1 に対応) :
共分散予測 (式 2 に対応) :
修正（アップデート）
フィルター ゲイン (式 3 に対応) :
ここでは 2 つのモジュールに分割しています。1 つのモジュールは反転を計算し、もう 1 つのモジュールはフィルター ゲインを計算します。
ステータス更新 (式 4 に対応) :
共分散更新 (式 5 に対応) :
要約する
ここのサブシステム入力には、入力データをターゲット行列に再構築するための Reshape モジュールがあります。変数を初期化する必要がある場合は、IC モジュールを使用して入力を初期化できます (IC モジュールはモデルの最初のステップでのみ入力されます)元のフィードバック入力)、データについては、データ ディクショナリ sldd を使用して信号線をバインドし、モデル内で外部から呼び出されるグローバル変数として sldd 内のデータのコード生成を設定できます。エクスプローラー オプションにより、埋め込みコードが生成されるときに変数も呼び出すことができます。フィードバック ループを接続するとき、直接フィードスルー信号の場合は、1 ステップ遅延モジュールを追加する必要があります。そうしないと、モデルの実行がすべてのモジュールで同時に計算されるため、代数ループが生成され、埋め込みコードを生成できません。フィードバック制御システムでは、因果関係システムにも従わなければなりません。
S-Function-Builder
当初は S-Function を使用してモデリングとシミュレーションを行っていましたが、シミュレーション後、埋め込みコードを生成してチップに焼き付けようとしましたが、埋め込みコードが生成できないことがわかり、S-Function を使用する必要がありました。代わりに Function-Builder を使用します。実際、2021 バージョンの Simulink で S-Function-Builder を使用することは、以前のバージョンよりもはるかに便利ですが、ライブラリをインポートするときやヘッダー ファイルを呼び出すときなど、理由もなく落とし穴もいくつかあります。コードをコンパイルすると、エラーを報告し、パスを変更しても解決しません。その後再起動すると、ソフトウェアは実際に再び動作します。まず、完全なモデル図をアップロードします。
完全なモデル図
単純なサブシステム モデリングよりもはるかに優れているように見えますが、とにかくそう思います。アルゴリズム モデルは、.c ファイルを使用して S-Function-Builder に完全に実装されます。
主なアルゴリズムの実装は、(S-FunctionName)_Outputs_wrapper 関数に実装されます。
void KF_Outputs_wrapper(const real32_T *u0,real32_T *y0)
{
/* Output_BEGIN */
// Update Data
X[0] = u0[0];
X[1] = u0[1];
X[2] = u0[2];
Y[0] = u0[3];
Y[1] = u0[4];
/* Calc Start */
// X_hat = F * X;
matrix_mul(3,3,1,F,X,X_hat);
// P_hat = F*P*F_T + Q;
matrix_mul(3,3,3,F,P,FP);
matrix_mul(3,3,3,FP,FT,FPFT);
matrix_add(3,3,FPFT,Q,P_hat);
// P_inv = H*P_hat*H_T + R;
matrix_mul(2,3,3,H,P_hat,HPhat);
matrix_mul(2,3,2,HPhat,HT,HPhatHT);
matrix_add(2,2,HPhatHT,R,P_inv);
// K = P_hat*H_T*inv_P;
Gauss((float(*)[2])P_inv, (float(*)[2])inv_P, 2);
matrix_mul(3,3,2,P_hat,HT,PhatHT);
matrix_mul(3,2,2,PhatHT,inv_P,K);
// X = X_hat + K*(Y - H*X_hat);
matrix_mul(2,3,1,H,X_hat,HXhat);
matrix_sub(2,1,Y,HXhat,Y_HX);
matrix_mul(3,2,1,K,Y_HX,KY);
matrix_add(3,1,X_hat,KY,X);
// P = (I - K*H) * P_hat;
matrix_mul(3,2,3,K,H,KH);
matrix_sub(3,3,I,KH,I_KH);
matrix_mul(3,3,3,I_KH,P_hat,P);
y0[0] = X[0];
y0[1] = X[1];
y0[2] = X[2];
/* Output_END */
}
システムの入力は u で、出力は y です。
要約する
サブシステム モジュールによって構築されたモデルと比較して、モデル全体ははるかに直感的であり、最終出力を計算するために何度も周回する必要はありません。S-Function-Builder によって生成されたコードを使用して得られる結果は、基本的に元のアルゴリズムの結果と同様であるため、最適化のためにフィルタリングと速度補償を採用しました。KFアルゴリズムでは、融合した速度と期待するベンチマークとの間に誤差があり、QであってもRであっても影響を及ぼしますので、データが乖離しておらず信頼できるものであることを前提に速度を補正しています。 . 、初期化時には 0 に基づきます。同時に、加速度センサーのデータジッターは非常に明白であるため、フィードバックパスの前に重み付き再帰フィルタリングアルゴリズムが追加され、出力データを処理するためにLPFが後ろに追加され、抑制効果が高まります。発振は特に顕著ですが、フィルタリングによりデータと元のデータの間に位相のずれが生じます（応答が遅くなります）が、データの信頼性の観点から、データの信頼性と引き換えに応答を少し犠牲にします。